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ACSTRACT 


The  performance  of  three  extended  Kalman  filter  implementations 
that  estimate  target  position,  velocity,  and  acceleration  states  for  a 
laser  weapon  system  are  compared  using  various  target  acceleration  tra¬ 
jectories.  Measurements  available  to  the  extended  Kalman  filters  each 
update  are  taken  directly  from  the  outputs  of  a  forward  looking  infrared 
(FLIR)  sensor.  Two  dynamics  models  considered  for  incorporation  into 
the  filter  are  1)  a  Brownian  motion  (BM)  acceleration  model  and  2)  a 
constant  turn  rate  (CTR)  target  dynamics  model.  The  CTR  filter  was 
compared  against  the  CM  filter  to  see  if  the  more  complex  dynamics  of 
the  CTR  filter  gave  it  a  significant  improvement  in  tracking  performance 
over  the  CM  filter.  These  two  simple  extended  Kalman  filters  were  then 
compared  to  a  multiple  model  adaptive  filter  consisting  of  a  bank  of  three 
filters  based  on  the  Brownian  notion  acceleration  model.  All  three  filters 
are  tested  using  three  different  flight  trajectory  simulations:  a  2  g, 
a  10  g  and  a  20  g  pull-up  maneuver.  All  evaluations  are  accomplished  using 
Monte  Carlo  simulation  techniques. 

The  constant  turn  rate  extended  Kalman  filter  was  found  to  outperform 
the  other  two  filters.  The  main  advantage  this  filter  had  was  the  minimi¬ 
zation  of  mean  bias  error  in  estimating  position.  The  standard  deviation 
of  error  was  also  slightly  lower  in  most  instances. 
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I.  INTRODUCTION 


Background 

Low  and  moderate  energy  lasers  have  made  important  contributions 
in  numerous  applications  including  medicine,  science,  cartography, 
communications,  range  finding  and  target  designation.  The  Air  Force 
Weapons  Laboratory  (AFWL)  located  at  Kirtland  Air  Force  Base,  New 
Mexico,  has  demonstrated  the  effectiveness  of  high  energy  lasers  for 
use  as  a  weapon  in  both  stationary  target,  on-ground  tests,  and  air-to- 
air  tests  using  the  airborne  laser  lab.  Lasers  have  many  unique 
characteristics,  chief  among  these  being  the  speed  of  light  at  which 
energy  is  transmitted  from  source  to  destination,  virtually  eliminating 
the  need  to  "lead"  the  target.  An  aircraft  ‘Hying  at  twice  the  speed 
of  sound  will  only  travel  one-eighth  inch  in  the  time  it  takes  laser 
energy  to  travel  one  mile. 

Key  components  of  a  laser  weapon  system  include  both  the  laser 
itself,  which  generates  the  high  power  light,  and  the  beam  control  sub¬ 
system,  which  aims  the  laser  beam  at  the  target  and  focuses  It  on  a 
vulnerable  spot  on  the  target.  The  optics  control  (pointing)  and 
target  position  estimation  have  to  be  very  accurate  in  order  to  maintai 
the  laser  beam  on  a  specific  part  of  the  target  long  enough  to  incapa¬ 
citate  a  vital  component  of  the  vehicle.  "Painting"  the  entire  target 
with  laser  energy  is  inefficient  and  would  require  extremely  high 
amounts  of  energy  to  achieve  destruction.  Elements  of  a  high  energy 
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laser  system  can  be  seen  in  Fijure  1  (Ref  1). 

The  research  In  this  report  is  concerned  only  with  the  "fine 
track"  portion  of  the  laser  weapon  system. 

Problem 

AFl.'L  is  interested  in  tracking  air-to-air  missiles  at  close  ranges 
to  accuracies  better  than  achievable  with  the  tracking  algorithms  cur¬ 
rently  in  use.  The  objectives  of  this  research  were:  (l)Design  a  Con¬ 
stant  Turn  Rate  (CTR)  dynamics  model  and  compare  the  filter  based  on  this 
truer  acceleration  model  against  a  previously  designed  Brownian  motion 
(Bf‘)  dynamics  model.  The  purpose  was  to  determine  whether  or  not  a  sub¬ 
stantial  improvement  in  performance  would  be  gained  by  going  to  the  more 
complex  acceleration  model;  (2)Design  and  test  a  multiple  model  adaptive 
filter  for  use  as  a  laser  tracker.  The  multiple  model  adaptive  filter 
was  designed  to  select  the  best  tuned  filter  from  a  bank  of  filters  to 
"fine  track"  a  target  in  any  one  of  a  range  of  trajectories  from  flying 
straight  and  level  to  pulling  high  "g"  turns.  This  bank  of  specialized 
or  fine-tuned  filters  was  then  compared  against  the  all  purpose  BH  and 
CTP.  filters  to  determine  whether  or  not  a  substantial  performance  improve¬ 
ment  would  be  gained. 

Previous  Investigation 

In  1978,  Captain  Daniel  Mercier  completed  an  initial  thesis  which 
demonstrated  the  feasibility  of  using  an  Extended  Kalman  Filter  (EKF) 
designed  to  track  benign  targets  using  outputs  from  a  forward  looking 
Infrared  (FlIR)  sensor  as  measurements.  It  exploits  knowledge  unused  by 
current  correlation  trackers— size,  shape,  motion  characteristics  of  the 
target,  and  atmospheric  jitter  spectral  description  to  yield 
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Figure  1.  Elements  of  a  High  Energy  Laser  System 


Improved  performance  (Ref  2). 

Captains  Robert  Jensen  and  Douglas  Harnly  continued  the  research 
In  1979.  In  order  to  track  air-to-air  missiles  at  close  range,  the 
algorithm  they  developed  Incorporated  on-line  adaption  to  target  shape 
effects,  changing  target  motion  characteristics,  and  maximum  signal 
Intensity.  The  algorithm  was  shown  to  possess  considerable  performance 
potential  for  highly  maneuverable  targets  despite  background  clutter 
by  Incorporating  some  ad  hoc  maneuver  indicators. 

Good  tracking  performance  was  achieved  on  the  basis  of  estimat¬ 
ing  target  velocity  and  acceleration  as  well  as  position.  Elliptical 
constant  Intensity  profile  contours  with  major  axis  aligned  with  the 
estimated  velocity  vector  were  assumed  (see  Figure  2).  Adaptively 
estimating  the  maximum  intensity  jn  the  FLIR,  the  major  and  minor  axis 


Figure  2.  Target  Image  in  FLIR  Field  of  Yiew 
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magnitudes  of  the  ellipse,  and  the  strength  of  the  dynamic  driving 
noise  contributed  to  the  good  tracking  performance  that  was  achieved. 

Oue  to  assumed  zero-mean  acceleration  models,  some  rather  tenuous  ad  hoc 
modifications  were  required  to  track  the  more  dynamic  targets  (Ref  5). 

The  truth  model  and  filter  used  by  Jensen  and  Harnly  Clef  3)  will 
serve  as  a  beginning  for  this  research. 

Assumptions 

FLIR.  The  FLIR  outputs  are  instantaneous  samples  of  an  array  of 
infrared  detectors  which  are  mechanically  scanned  by  a  many  facetted 
mirror  through  a  restricted  field  of  view.  Each  minute  infrared 
detector  emits  electrical  current  proportional  to  the  average  intensity 
of  the  infrared  photons  entering  the  face  of  the  detector.  A  single 
digitized  output  represents  a  real  time  electronic  spatial  average  of 
a  horizontally  scanned  detector.  The  serial  digitized  data  can  either 
be  stored  or  displayed  on  a  cathode  ray  tube  (CRT);  each  picture 
element  of  the  CRT  is  called  a  pixel.  The  horizontal  scanning  of  the 
detectors  proceeds  vertically  through  the  FLIR  field  of  view  resulting 
in  an  array  (frame)  of  pixels  which  is  analogous  in  size  (about  500  by 
400  elements)  and  appearance  to  a  normal  TV  picture.  A  new  frame  of 
pixels  is  generated  every  thirtieth  of  a  second  (30  Hz  frame  rate). 

For  this  study,  an  8-by-8  array  of  pixels  out  of  each  large  frame  of 
pixels  constitutes  a  single  measurement  array  (often  called  a  "tracking 
window")  for  processing  by  the  proposed  filter  algorithms.  Restricting 
the  size  of  the  measurement  array  is  primarily  dictated  by  computational 
and  storage  limitations  and  partially  justified  by  fast  measurement 
rates.  (Ref  3:4-5) 
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Target.  The  target  for  some  ARIL  tests  and  this  research  Is  an 
air-to-air  missile.  A  bivariate  Gaussian  Intensity  profile  with  ellip¬ 
tical  equal-intensity  contours  with  specified  angular  orientation  is 
used.  Real  FLIP  data  supports  the  use  of  this  model  to  approximate 
closely  missile  shape  in  the  image  plane  (FUR  focal  plane)  (Ref  3:5). 

The  apparent  target  intensity  function  location  in  the  FUR 
field  of  view  consists  of  several  components.  Boresight  error,  FLIP 
system  vibrations  and  others  are  assumed  to  be  negligible  so  that  the 
intensity  function  location  can  be  centered  by: 

xpeak(t>  +  xA(t) 

s 

ypeak(t)  yD(t)  +  y/\(t) 


where  xD,  yD  ■  position  offsets  due  to  target  dynamics 

XA*  yA  =  Posit^011  offsets  due  to  atmospheric  jitter 

Atmospheric  jitter  was  modeled  as  a  first  order  Gauss-f’arkov  process, 
the  output  of  a  first  order  lag  driven  by  white  Gaussian  noise,  as  follows 


xA(t)  =  -  -  xaW  + 

TA 

where  w(t)  is  an  independent  white  Gaussian  noise  process  and  is  the 
correlation  time  of  atmospheric  jitter,  1/14.14  seconds.  This  model  was 
used  in  both  the  x  and  y  coordinate  directions. 

Background  Noise.  The  image  background  noise  which,  along  with 
FLIR  sensor  noise,  contaminates  the  FLIR  measurements,  is  modeled  as  a 
spatially  and  temporally  correlated,  Gaussian  process.  Various  physical 
backgrounds  result  in  FLIR  images  with  differing  spatial  and  temporal 
correlation  characteristics.  Real  data  analysis  and  ARIL  experience 
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have  been  used  to  determine  appropriate  correlation  coefficients  for 
the  spatial  and  temporal  correlations. 

Closed  Loop.  This  is  a  closed  loop  tracking  system.  The  laser 
pointing  system  is  assumed  to  be  perfect.  That  is,  the  system  can 
point  exactly  where  the  tracker  commands  it  within  each  sample  period. 
This  implies  that  settling  time  of  the  pointing  system  is  less  than 
the  data  sample  period,  the  1/30  sec  time  between  discrete  samples  of 
the  FLIP,  output  (Ref  3:8). 
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II.  SYSTEM  DYNAMIC  MODELS 


General 

Two  different  dynamic  models  were  used  in  the  bank  of  filters 
needed  for  the  multiple  model  adaptive  estimation  algorithm.  The 
first  model*  a  Brownian  motion  acceleration  model*  was  developed  by 
Captains  Hamly  and  Jensen  (Ref  3).  The  second  model,  a  constant 
velocity,  constant  turn  rate  model,  was  developed  by  Lieutenant  Ruoff 
(Ref  6)  and  this  author. 

An  extended  Kalman  Filter  (EKF)  algorithm  was  used  for  filter 
propagation  and  update  since  both  filter  types  had  nonlinear  dynamics 
and/or  measurement  models.  This  algorithm  provides  a  new  reference 
state  trajectory  each  time  new  st2te  estimates  are  calculated. 

Brownian  Motion  Dynamics  Model.  The  Brownian  Motion  (BM) 
dynamics  model  development  can  be  found  inMarnley  and  Jensen's  thesis 
(Ref  3)  and  will  not  be  duplicated  here.  Only  the  results  will  be 
given. 

The  following  BM  dynamics  model  was  used: 


X 

vx 

y 

vy 

vx 

«x 

ax 

(1)  X  - 

ay 

"x 

\y 

(*1/ta)  a^  +  wAx 

1  ' 

_H/TA}  aty  +  \ 

or 

x  -  F{t)x(t)  +  G(t)w(t)  (3) 

where  £(t)  and  G(t)  can  be  deciphered  from  Eq.  (2),  and  only  the  x  and 
y  coordinates  are  used  since  the  data  is  taken  off  the  FLIR  image  plane, 
where 

x  *  azimuth  position 
y  *  elevation  position 
vx  *  azimuth  velocity  ■  x 
Vy  °  elevation  velocity  ■  y 
a„  *  azimuth  acceleration  ■  v„ 
a„  ■  elevation  acceleration  »  v 

y  y 

at  «  azimuth  atmospheric  disturbance  (jitter) 
x 

a+  *  elevation  atmospheric  disturbance 

tt 

«  correlation  time  of  atmospheric  jitter 
w  »  zero  mean  white  Gaussian  noise 

ELwi (t)  Wf(s)J  -  a^ft-s)  i  *  x,y,ax,ay 

These  eight  state  variables  are  related  to  the  output  measurements 
z  by  means  of  a  nonlinear  function. 

ifV  *  jl L^V't*]  ♦v(t.)  (4) 

where  v(«)  Is  a  discrete-time  white  Gaussian  noise  sequence  of  zero  mean 
and  covariance  kernel 

E  [vU-f)  IT(tjJ  -  (5) 
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0  At 


0 


0 


0  At2/2  0 

0  1  0  At  0  At2/2  0  0 

0  0  1  0  At  0  0  0 

0  0  0  1  0  At  0  0 

*  -  00001000  (9) 

0  0  0  0  0  1  0  0 

000000  A1  0 

0000000  A1 

A1  3  exp  (-At/raJ 


where 


A1 

-  At5oD2/20 

A5 

«  At2cD2/2 

A2 

-  At4oD2/8 

A6 

*  4toD2 

A3 

*  At20Q2/6 

A7 

*  oA2[l-exp(-2At/TA3 

A4 

=  At3oD2/3 

D  is 

the  RMS  value 

of  target  motion  and 

aA  is  the  RMS  value 

of  the  atmospheric  jitter. 

The  measurement  update  relations  for  an  EKF  using  the  inverse 


covariance  form  are: 

P'VJ)  =  P_1U-)  +  HT  ( t^ )  RT1  ( ti )  H.(  t7. )  (13) 

P(t{)  -  p-’ttf)  (14) 

Kd^)  •  P.(  t^)Hj(  ^  JR-1  ( tt )  (15) 

x(t+)  =  x(t~)  +  K( ti ) [r_( tn- )  -  h(x(tT),  (16) 


Where  v_{ t.)  is  a  sample  of  z_  at  time  tj.  R_is  the  covariance  of  the 
measurement  model  and  is  a  constant.  The  £(tj)  matrix  is  defined  as: 
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x=£(t-) 


07) 


ax. 


For  details  of  how  this  derivation  is  computed,  see  Reference  3 
and  program  listing  (subroutine  MEASF)  found  in  Appendix  B, 

Constant  Turn  Rate  Dynamics  Model 

The  Constant  Turn  Rate  (CTR)  dynamic  model  differs  from  the 
Brownian  motion  dynamic  model  in  the  handling  of  the  jerk  level  motion 
(time  derivative  of  acceleration).  The  BM  model  expects  zero  mean 
acceleration  with  the  jerk  motion  modelled  as  white  noise.  The  CTR 
model  replaces  this  jerk  model  with  one  that  corresponds  to  a  planar, 
constant  velocity,  constant  turnrate  maneuver,  typical  of  the  behavior 
of  airborne  targets. 

The  acceleration  model  was  given  by: 

i  *  in 2 v.  +  w  (18) 

where 

v^  =  velocity  in  the  FUR  image  plane 
w  *  zero  mean  white  Gaussian  driving  noise 
til  ■  magnitude  of  the  target's  turn  rate;  Eq,  (28)  shows 
how  6)  was  calculated. 

Eq.  (18)  was  developed  from  the  application  of  the  Coriolis 
theorem  written  as: 

*d(v)  .  Td(vJ  ’  *wT  *vT  (19) 

~dt~  '  ir- 

where  "x"  denotes  the  cross  product  and  "I"  and  "T"  represent  the 
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Inertial  and  target  coordinate  frames  respectively.  The  FLI R  image 
plane  is  treated  as  essentially  inertial  compared  to  the  rotating 
target  frame.  The  inertial  frame  origin  is  located  at  the  tracker  with 
"y"  coordinate  measuring  altitude.  The  origin  of  the  target  frame  is 
centered  on  the  target  (See  Figure  3).  On  d(  )/dt,  an  "I"  or  "T"  super¬ 
script  means  as  seen  from  that  frame. 

Note:  V  and  *vj  are  the  inertial  target  angular  velocity  and 
inertial  target  velocity  respectively,  and  for  rotational  simplicity 
are  shortened  to  cu  and  v. 
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Since  the  target  is  assumed  to  be  of  constant  speed  the  first 
term  on  the  right  hand  side  is  zero.  Now  the  derivative  of  Eq.  (19) 
with  respect  to  time  gives 

!d2  (v)/dt2  =  Id(co  x  v)/dt  (20) 
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or  expressed  in  the  target  body  frame: 

*d^  (vj/dt^  ■  x  v)/dt  +  (u  x  (w  x  v))  (21) 


Mow  since  both  target  speed  and  angular  velocity  were  assumed 
constant  Eq.  (19)  becomes: 

*d^  (v)/dt  =  (oj  x  (w  x  v)  (22) 


Using  the  relationship  for  a  triple  cross  product,  Eq.  (22)  can  be 
written: 


*d^  (v^/dt^  =  (w  •  v):o  -  (co  •  w) v_  (23) 

The  first  term  of  Eq.  (23)  is  zero  since,  for  a  planar,  constant 
angular  rate,  constant  speed  turn,  the  target's  inertial  velocity 
and  angular  velocity  vectors  are  perpendicular.  Thus,  Eq.  (23) 
becomes : 

1d2  (v)/dt2  =  .w2v  (24) 

which  is  the  same  as  Eq.  (18)  with  white  noise  to  model  the  effects 
of  modeling  approximations  and  real  world  disturbances. 


(25) 


or  equivalently: 

| vxay  -  vyax 

<o  * 


(26) 
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The  filter  states  are  the  same  eight  states  used  In  the  BM 
model,  but  the  dynamics  model  Is  now  nonlinear. 

x(t)  =  f(x(t),t}  +  Gw(t)  (27) 


x(t)  * 


x 

vy 

ax 

a.. 


-to2vx  +  v/x 

-C02Vy  +  Wy 
-VtA  +  Wflx 
-l/T/\,  +  WAy 


(28) 


with  to  given  as  in  Eq.  (26).  The  output  measurements  are  taken  as 
they  were  in  the  first  filter. 

The  time  propagation  equations  can  be  written  as; 


t/ti )  ,tj  dt 


(29) 


or  since  £(t/t^)  ■  f[S(t/t^ ) ,t)  numerically  integrating  Eq.  (29) 
using  a  first  order  Euler  integration  technique  yields: 

$(t}+1)  *  £Ui)  +  i(t,)At 

-  i(t|)  +  f[l(tt),  tj  At 


This  approximation  is  valid  as  long  as  At  is  small  enough  so 


that  second  order  terms  and  higher  can  be  neglected.  This  first 
order  approximation  Is  also  used  In  writing  the  matrix,  as  will  be 
seen  In  Eq.  (33). 

The  covariance  propagation  is  the  same  as  before  only  now 
£(t.j+At,  t^)  must  be  calculated  every  propagation: 

£(ti  +  At,  ti)  =  exp(-F(tj )At)  (31) 


which  is  a  quasi-static  approximation.  It  assumes  that  £  is  constant 
over  the  duration  of  the  sample  period.  This  approximation  is  good 
as  long  as  the  sample  period  is  short  as  compared  to  the  rate  at  which 
£  changes.  The  £  matrix  Is  generated  using  the  relation: 


3f 


3x 


x  =  X(tt) 


(32) 


The  upper  left  6-by-6  partition  of  £(t^  +  At,  t^)  is  approximated 
with  a  first  order  Taylor  series  expansion.  The  correct  closed  form 
solution  was  easy  to  obtain  for  thc  two  atmospheric  disturbance  states  so 
truncating  a  Taylor  series  expansion  to  first  order  terms  was  not  necessary. 
The  closed  form  was  used  for  those  two  states.  The  equation  used  for  the 
elements  associated  with  the  'irst  six  states  was : 


£(ti  +  At,  ti)  =  l  +  F(ti)At 


(33) 
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where 

_I_  =  e  oxG  identity  ratrix 

At  =  a  sample  period,  1/30  second 
The  derivation  of  £(t^)  can  he  found  in  Appendix  A. 

Since  this  program  has  to  operate  in  real  tine  with  each  sample 
period  lasting  under  1/30  sec  (At),  a  number  of  simplifications  in  the 
form  of  first  order  approximations  were  made.  As  mentioned  previously, 
the  matrix  was  also  simplified  in  this  manner.  This  term  represents 
the  degree  of  uncertainty  present  in  the  filter  propagation.  In  the 
case  of  a  perfect  dynamics  model  with  no  driving  noise  would  he  zero, 
reflecting  absolute  certainty  in  the  estimate  during  the  time  between 
measurements.  On  the  other  hand,  a  poor  dynamics  model  would  require 
to  have  large  values.  The  CTR  model  is  believed  to  be  a  truer  model 
of  what  the  target  dynamics  are,  so  entries  will  be  smaller*,  because 
of  this  and  a  short  sample  period,  a  first  order  matrix  approximation 
is  warranted.  Thus,  Eq.  (14)  is  approximated  by  ignoring  higher  order 
terms  to  yield 

oocono  oo 

oooooo  oo 

0  0  00  0  0  00 

5^*0  o  o  o  o  o  o  o 

0  0  0  0  A6  0  0  0 

0  0  0  0  0  A6  0  0 

000000  A7  0 

000000  0  A7 

where  A6  and  A7  are  the  same  as  before. 
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This  approximation  also  avoids  the  need  for  recomputing  each  sampl 
period,  which  would  be  necessary  because  *  changes  each  sample  period 
(Eqs.  (10),  (32),  and  (33)). 


III.  Multiple  Model  Filtering  Algorithm 


Theory 

The  Multiple  Model  Filtering  Algorithm  is  an  adaptive  filter  com¬ 
posed  of  a  bank  of  K  separate  Kalman  filters,  in  this  case,  extended 
Kalman  filters,  each  based  on  a  particular  value  a^,  a^,  .  .  . ,  or  a^  of 
the  parameter  vector  (Ref  4:  Ch  10,  103-110).  The  basic  structure  is 
shown  in  Figure  4. 

This  algorithm  is  based  on  magnitudes  of  time  histories  of 
residuals  rj(t^),  rg(tj)»  .  -  .,  r^tj)  generated  in  the  K  filters 
when  the  measurement  becomes  available.  The  residuals  are  processed 
by  the  hypothesis  conditional  probability  computation  shown  in  Eq.  (34) 
and  in  turn  the  pk(t.)  values  are  used  as  weighting  coefficients  to 
generate  £(tj)  as  seen  in  Eq.  (35) 

fl(ti)|a,Z(ti_i)  (-i  Lik’  Ii-1^  Pv^i-l^ 

p)c(ti)  =  — - : - - -  (34) 

K 

^(Vl^ti.-i)  Pi  (t1-l> 

xUj*)  *  l  x^tj)  Pk(ti)  (35) 

where  x^t*)  is  the  state  estimate  produced  by  a  Kalman  filter  based  on 
the  assumption  that  the  parameter  vector  equals  ak.  Therefore,  the  over¬ 
all  state  estimate  is  the  probabilistically  weighted  average  of  the  state 
estimate  generated  by  each  of  K  separate  Kalman  filters. 
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The  conditional  density  function  used  in  Eq.  (34)  can  be 
evaluated  as 


fz(ti)|a,Z(ti-i)(£i/£k*Ii-i)  *  XV)m/z|Ak(ti)|}s 


(36) 


-’EkUf  JAjJ1  (t|  ^(t^ 


where  A^t^)  is  generated  in  the  k-th  Kalman  filter  as 

A^)  *  Hfcd^jPfed^-jHfcTd^)  +  ^(ti)  (37) 

and  the  residuals  were  calculated  in  each  filter  as 


rk(ti )  *  z^)  -  hjcfxjcUrMi) 


(38) 


If  it  is  desired  to  produce  an  estimate  of  the  parameter  vector 
itself,  the  conditional  mean  of  <i  at  time  tj  Is 
A-  K 

atti)  «  z  akpk(t1)  (39) 

k«l-*  k 

The  residuals  of  the  Kalman  filter  based  upon  the  "correct" 
model  are  expected  to  be  consistently  smaller  (relative  to  the  filter' 
internally  computed  rms  residual  values)  than  the  residuals  of  the 
other  mismatched  filters.  If  this  is  true,  then  equations  (34)  and 
(36)  will  cause  the  "correct"  probability  pk(tj),  i.e.,  the  one  whose 
index  Is  associated  with  the  correct  filter  model,  to  Increase,  while 
causing  the  others  to  decrease. 

The  performance  of  this  algorithm  is  dependent  upon  significant 
difference  between  the  residual  character! sti cs  in  the  "correct"  and 
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the  "mismatched  model"  filters.  If  the  residuals  are  consistently  of 
the  same  magnitude,  then  Eq.  (34)  and  Eq.  (36)  result  In  the  growth  of 
the  pk  associated  with  the  filter  with  the  smallest  value  of  which 
Is  Independent  of  the  "correctness"  of  the  models  and  as  such  a  result 
would  be  totally  erroneous  (Ref  4:  Ch  10,  106). 

Multiple  Model  Algorithm  Implementation 

Both  the  constant  turn  rate  and  Brownian  motion  acceleration 
models  were  considered  for  inclusion  into  the  bank  of  filters  required 
for  the  multiple  model  adaptive  filtering  algorithm.  For  the  initial 
design  and  testing,  it  was  decided  to  use  only  one  type  of  filter  in 
the  filter  bank.  This  decision  was  made  because  of  the  complexities 
involved  in  integrating  two  or  more  different  types  of  filters  into  the 
program  used  and  because  of  time  constraints  in  completing  this  project. 
The  CTR  filter  was  found  to  be  good  at  tracking  targets  over  a  wide 
dynamic  range  when  properly  tuned.  This  capability  made  it  unsuit¬ 
able  for  testing  the  multiple  model  algorithm  as  the  residuals  would  be 
very  similar  for  a  bank  of  CTR  filters.  A  Brownian  motion  filter 
tuned  to  a  specific  target  trajectory  had  a  smaller  dynamic  range  than 
the  CTR  filter  in  which  it  outperformed  other  BM  filters  tuned  to  other 
trajectories.  Thus,  it  lent  itself  better  to  this  application  than  the 
CTR  filter. 

In  this  case,  associated  with  each  a^  was  a  different  system  model, 
differing  in  their  assumed  Q.  values.  Three  system  models  were  used, 
three  Brownian  motion  filters  tuned  to  a  2g  turn,  a  lOg  turn,  and  a  20g 
turn. 

A(t^)  is  a  64x64  matrix  and  Eq.  (36)  calls  for  the  inverse  of 
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A(t^)  In  the  calculation  of  p^t^).  This  matrix  Inversion  would 
use  more  computer  time  than  available  between  updates,  and  so  an 
approximation  to  A”^(t^)  was  mati.  One  option  considered  to  avoid 
a  full  matrix  inversion  was  to  use  only  the  64  elements  of  A(t.j) 
corresponding  to  the  center  4x4  array  of  the  FUR  (fovea!  area) 
since  this  Is  where  the  target  Image  is  expected  to  be  reflected 
most  of  the  time.  The  full  Inversion  would  then  be  taken  of  either 
of  these  8x3  arrays  and  the  elements  then  placed  on  the  diagonal 
of  a  64x64  array.  The  approximation  chosen  was  to  use  only  the 
diagonal  of  A(t^),  calculated  in  Eq.  (37),  in  calculating  the  in¬ 
verse.  This  was  the  easiest  algorithm  to  implement  and  used  the 
smallest  amount  of  computer  time.  A  full  inversion  would  require 
more  than  32  million  multiplies  for  each  filter  every  sample  per¬ 
iod,  an  8x8  inversion  requires  approximately  6500  multiplies  and 
about  one  thousand  additions  and  subtractions.  The  64x64  diagonal 
matrix  chosen  requires  only  63  multiplies  and  64  divisions.  This 
approximation  greatly  decreases  the  amount  of  time  required  for  each 
inversion. 

The  determinant  of  Ajj(t^)  required  in  Eq.  (36)  would  require 
over  8000  multiplies  and  so  an  approximation  would  be  needed  here 
too.  Since  all  of  the  filters  were  of  the  same  form,  the  ( t^ ) ' s 
were  similar  while  the  major  difference  was  in  the  residuals,  so  the 
scalar  (2tr )m/2  Aj^tj)*^  terms  were  ignored  in  Eq.  (34).  The  approx¬ 
imation  that  would  have  been  used  to  evaluate  AjJt^)  also  contri¬ 
buted  to  the  decision  to  ignore  the  terms  because  of  similarity. 

Due  to  the  size  of  the  matrices  Involved,  the  product  rjJ(t^) 

(ti )lk(ti )  often  exceeded  ^480  which  was  too  large  to  use  as  the 
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argument  for  the  exponential  function  and  exceeded  the  computer's 
upper  numerical  bounds.  A  scale  factor  of  .01  v/as  used  to  bring 
the  product's  magnitude  down  to  acceptable  levels.  This  also  changes 
the  magnitude  of  the  ratio  of  one  filter  to  another.  This  was  assumed 
to  be  acceptable  (Ref  7)  until  a  better  method  of  scaling  the  exponent 
could  he  found.  The  residuals  from  the  "foveal  region"  and  the  optional 
0x2  Ajtj)  matrix  mentioned  above  could  have  been  used  so  that  scaling 
might  not  have  been  necessary. 

The  final  form  of  !iq.  (3C)  implemented  was: 


The  values  from  Eq.  (34)  were  lower  bounded  to  keep  the  pro¬ 
bability  factor  from  going  to  zero  and  to  keep  the  reaction  time, 
necessary  to  adjust  to  a  new  target  trajectory,  at  acceptable  levels. 
The  minimum  value  of  pk  was  chosen  to  be  .01.  This  choice  was  based 
on  test  results  of  the  program  itself. 
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IV  Method  of  Evaluation 


Monte  Carlo  Simulation 

Modified  versions  of  an  existing  digital  simulation  program  (Pef  3) 
were  used  to  test  each  of  the  three  filter's  performance.  The  propagation 
of  the  filter's  state  estimates  and  covariance  portions  of  the  program 
were  modified  to  reflect  either  a  Brownian  motion  or  constant  turn  rate 
extended  Kalman  filter.  Additionally,  the  program  was  modularized  for 
the  adaptive  multiple  model  extended  Kalman  filter  in  order  to  take 
advantage  of  sections  of  the  program  that  are  common  to  all  filters.  This 
avoids  having  to  duplicate  large  sections  of  code. 

The  outputs  from  this  program,  the  truth  model  state  vector,  the 
filter's  estimate  of  the  state  vector,  and  the  filter's  covariance  matrix 
for  each  interval,  were  stored  for  post-processing.  A  statistical  eval¬ 
uation  and  plotting  routine  developed  by  Captain  Mercier  (P.ef  2)  was 
used  to  do  the  post-processing. 

The  number  of  Monte  Carlo  runs  made  through  each  simulated  trajec¬ 
tory  for  each  filter  was  20.  This  number  was  selected  to  provide  con¬ 
fidence  in  the  accuracy  of  the  solution.  Captains  Harnley  and  Jenson 
showed  that  20  runs  was  sufficient  in  their  thesis  (P.ef  3)  and  was  veri¬ 
fied  for  this  work.  The  choice  of  20  passes  through  each  simulation  also 
kept  the  computer  execution  time  and  storage  requirements  at  acceptable 
levels. 

The  plots  available  from  the  plotting  routine  were  1)  the  mean  error 
between  the  truth  model  (target  trajectory  data)  value  and  the  corres¬ 
ponding  estimate  from  the  filter,  plus  and  minus  the  actual  standard  de¬ 
viation  as  generated  in  the  Monte  Carlo  analysis,  2)  the  actual  standard 
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deviation  versus  the  filter's  estimate  of  the  standard  deviation,  and 
3)  a  variance  convergence  plot  showing  whether  or  not  20  Ponte  Carlo 
simulation  runs  were  sufficient.  This  last  plot  showed  the  variance 
at  four  separate  points  in  time  in  the  simulation  as  each  Ponte  Carlo 
run  was  made.  As  more  passes  were  made  through  the  simulation,  the 
variance  at  each  point  in  time  leveled  off  indicating  convergence. 

The  first  two  plots  were  used  to  evaluate  filter  tuning  and  per¬ 
formance.  The  first  plot  was  used  to  check  the  mean  bias  and  peak 
errors  and  the  second  plot  was  used  to  compare  the  magnitudes  of  the 
standard  deviation. 

Trajectory  Generation  and  Description 

The  trajectories  used  in  the  evaluation  of  the  three  filters 
were  all  basically  identical  with  only  the  force  of  the  pull-up  man¬ 
euver  being  varied.  The  trajectories,  picked  to  represent  typical 
target  maneuvers,  all  started  with  a  constant  inertial  velocity  cross 
range  path  with  a  constant  multiple  *g'  pull-up  initiated  two  seconds 
into  the  simulation.  Each  engagement  lasted  five  seconds,  providing 
adequate  time  for  the  performance  evaluation  of  the  filters.  Figure  5 
shows  a  typical  trajectory. 


i 
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Before  the  pull-up,  the  equations  are 
x  *  -1000  m/sec 

y  -  0 
2  =  0 

x  =  6,000  m 
y  =  0 

2  =  40,000  in 

After  pull-up  initiation,  the  equations  are 

x  =  -1000  cos  aitt-2)  m/sec 
y  =  1000  sin  ai(t-2)  m/sec 
2=0 

x  =  -4000-(1000/w)sin(«u(t-2))  m 
y  =  (l  000/cj)  ( 1 .  -  cos  ai(t-2))  m 
2  =  40,000  n 

v/here  o  equals  0.0196,  .098,  or  0.196  radians  for  turn  magnitudes  of  2 
g's,  10  g*s,  and  20  g’s  respectively.  These  paths  were  intended  to  show 
the  ability  to  track  a  highly  maneuvering  missile. 

Figures  of  Verit 

The  plot  routine  printed  out  the  mean  error  plus  or  minus  the  stan¬ 
dard  deviation  of  the  error,  and  the  filter  estimate  of  the  standard  de¬ 
viation  versus  the  actual  standard  deviation  for  each  sample  period  us¬ 
ing  the  computer  average  of  the  20  f'onte  Carlo  runs.  This  was  done  for 
the  first  four  error  states,  the  x  and  y  coordinate  position  and  velocity 
errors.  This  data  was  then  used  to  find  the  time  averaged  mean  error 
and  the  time  averaged  variance  over  the  last  two  seconds  of  the  engage- 
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ment.  The  last  two  seconds  of  the  engagement  were  used  so  that  any 
initial  transient  effects  caused  by  the  change  in  dynamics  of  the 
maneuver  would  have  diminished.  This  provided  60  sample  points  (1/30 
second  sample  period),  each  point  being  an  average  of  20  f'onte  Carlo 
runs,  to  use  in  calculating  the  time  averaged  statistics.  Sixty  points 
was  felt  to  be  enough  because  of  the  Monte  Carlo  technique  used  in 
obtaining  each  point.  Also,  the  data  was  used  to  find  the  peak  error 
after  the  pull-up  maneuver  was  initiated. 

Again,  the  three  figures  of  merit  calculated  for  the  comparison 
of  the  Brownian  motion,  the  constant  turn  rate  and  the  adaptive 
multiple  model  filters  were: 

1)  the  time  averaged  value  of  the  x  and  y  mean  errors  in  position 
and  velocity  (all  rounded  to  three  decimal  places); 

2)  the  peak  value  of  the  x  and  y  mean  errors  in  position  and  ve¬ 
locity  (all  rounded  to  three  decimal  places);  and 

3)  the  time  averaged  value  of  the  standard  deviation  of  the  errors 
committed  by  the  filter  for  the  x  and  y  position  and  velocity  (all 
values  rounded  to  three  decimal  places). 

Samples  of  the  plots  used  in  tuning  the  Brownian  motion  and  constant 
turn  rate  filters  can  be  seen  in  Figures  6,  7,  and  8. 
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HERN  ERROR  «-l  SIGHA 
TARGET  DTNRHICS 
T  CHANNEL 


Y  CHANNEL  DYNAMICS  ERROR  CS/N=12.S) 
Figure  6,  Sauple  Output  from  Plotter  Routine 
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Deviation  of  Error 


RCTURL  VS.  FILTER 
SIGMA 
r  CHANNEL 
TARGET  OTNANICS 


FILTER  VS.  ACTUAL  SIGMA  PLOT  (S/N  =12.5) 

Figure  7.  Sanple  Output  from  Plotter  Routine 
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Figure  8.  Sacple  Output  from  Plotter  Routine 
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General  Performance  of  the  Three  Filters 

Before  the  direct  comparison  between  filters  is  made,  the  general 
performance  of  each  filter  will  be  discussed  to  provide  physical  in¬ 
sight  into  the  results.  Figures  C-l  to  C- 32  are  the  plots  of  the  re¬ 
sults  from  the  Monte  Carlo  simulations  of  the  Brownian  notion  (Bfl) 
extended  Kalman  filter  for  the  three  trajectories  used.  The  BM  filter 
was  characterized  by  oscillatory,  biased  estimates  in  both  of  the  FLIP 
x  and  y  axes  for  position  and  velocity.  The  largest  bias  errors  were 
in  the  x  position  and  y  velocity  estimations.  Also,  the  biases  in  the 
estimates  got  larger  as  the  pull-up  acceleration  was  increased.  These 
large  mean  errors  were  a  result  of  the  inadequacy  of  the  assumed  target 
relative  acceleration  model.  This  model,  a  Brownian  motion  process, 
did  not  accurately  model  the  target  acceleration  when  the  target  dy¬ 
namics  were  anything  but  straight  and  level  flight  as  was  the  case  for 
the  first  two  seconds  of  all  the  simulations  or  if  the  acceleration  was 
Brownian  motion.  Thus,  the  BM  filter  had  an  inherent  lage  in  the  esti¬ 
mates  of  the  states  when  unmodeled  maneuvers  with  persistent  turning 
accelerations  were  encountered.  The  high  measurement  update  rate  is 
what  kept  the  filter  from  diverging  and  losing  track  of  the  target 
while  the  unmodeled  maneuver  was  occurring. 

The  mean  errors  of  all  the  estimates  of  the  states  made  large 
excursions  outside  the  envelope  of  plus  or  minus  the  square  root  of 
the  corresponding  filter-computed  conditional  variance,  once  the  pull- 
up  simulation  started.  The  large  mean  errors  and  the  failure  of  the 
filter's  conditional  variance  to  reflect  the  growth  of  the  mean  errors 
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Indicated  that  the  filter  was  putting  too  much  weight  on  the  results 
from  the  dynamics  model  and  not  enough  weight  on  the  information  con¬ 
tained  in  the  measurements.  Further  tuning  of  the  DM  filter  or  the 
use  of  adaptive  Kalman  filter  techniques  (Ref  3)  would  help  alleviate 
this  problem. 

The  plots  of  the  results  from  the  Monte  Carlo  simulations  of  the 
constant  turn  rate  (CTR)  extended  Kalman  filter  are  presented  In  Fig  D-l 
to  D-40.  The  results  were  characterized  by  nearly  unbiased  estimates 
of  the  x  and  y  position  coordinates  in  the  FLIP  frame,  except  during 
Initial  transients  for  estimates  of  the  position  and  velocity.  The 
small  bias  values  observed  for  position  were  expected  since  the  tra¬ 
jectories  were  well  described  as  constant  turn  rate  trajectories  except 
during  the  intervals  of  rapid  acceleration  changes.  Large  mean  errors 
occurred  during  these  intervals  and  were  a  result  of  the  inadequacy  of 
the  assumed  constant  turn  rate  model  to  represent  the  rapidly  varying 
actual  target  acceleration.  The  mean  velocity  errors  observed  were 
much  larger  than  expected  and  larger  than  the  model.  The  cause  of 
this  was  unknown  and  was  ignored  since  the  position  was  the  critical 
performance  parameter. 

The  mean  errors  of  the  position  estimates  for  the  CTR  filter  were 
within  the  envelope  of  plus  or  minus  the  square  root  of  the  filter's 
corresponding  conditional  variance,  as  expected,  since  the  actual 
target  acceleration  was  the  same  as  the  assumed  model.  However,  the 
mean  errors  of  the  velocity  estimates  exhibited  large  biases,  as  pre¬ 
viously  mentioned,  that  exceeded  the  value  of  the  filter's  standard 
deviation.  One  method  that  might  eliminate  this  problem  would  be  to 
use  adaptive  Kalman  filter  techniques. 
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There  were  no  plots  generated  for  the  adaptive  multiple  model 
extended  Kalman  filter.  The  multiple  model  filter  was  not  able  to 
select  a  'best'  filter  from  the  bank  of  BP  filters  purely  on  the  basis 
of  the  residuals.  Instead  the  filter  adaptively  selected  one  filter 
and  stayed  with  it  throughout  the  simulated  maneuver.  The  software 
seemed  to  work  with  no  problems.  The  major  problem  was  the  multiple 
model  filter's  inability  to  select  one  CM  filter  over  another  because 
of  the  similarity  of  the  residuals.  Table  1  contains  the  statistics 
of  the  residuals  taken  from  a  simulated  20  g  turn  one  second  after  the 
turn  was  initiated. 


BM  Filter 

Mean 

Standard  Deviation 

1 

2 

-4.7353 

5.6497 

3 

-4.7257 

5.7227 

Table  1.  Residual  Statistics 


The  mean  of  the  residuals  differed  by  less  than  one-half  of  one  percent 
and  the  standard  deviations  differed  by  less  than  one  percent.  Thus 
at  any  given  sample  time,  any  one  of  the  three  filters  could  be  the  'best* 
.filter  on  the  basis  of  it's  residuals. 

Comparison  of  the  Three  Filters 

The  figures  of  merit  for  each  filter  for  the  three  trajectories  are 
presented  in  Table  2  and  Table  3.  As  seen  from  these  tables,  the  con¬ 
stant  turn  rate  filter  provided  estimates  of  position  in  the  10  g  and  20 
g  maneuvers,  where  the  time  averaged  scalar  magnitudes  of  the  mean  error 
were  significantly  better  than  the  Brownian  motion  estimates  at  high  turn 
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rates.  Also,  the  magnitude  of  standard  deviation  of  error  was 
slightly  better  in  the  CTR  filter  for  both  position  and  velocity  in 
most  cases. 

For  each  dynamics  model,  a  single  filter,  which  tracks  a  10  g  turn 
the  best,  was  used  as  a  baseline  against  which  to  tune  other  filters. 

This  filter  was  run  against  all  three  trajectories  and  then  it's  perfor¬ 
mance  was  the  yardstick  used  to  measure  how  well  other  filters  were 
tuned.  As  it  turned  out,  the  CTR  baseline  model  was  the  superior  filter 
for  the  2  g  and  10  g  case  and  was  very  close  in  performance  to  the  se¬ 
lected  20  g  CTR  filter.  The  baseline  BM  filter  also  was  the  best  tuned 
filter  for  two  trajectories,  the  20  g  and  10  g  case,  and  very  close  in 
performance  to  the  selected  2  g  filter.  The  BM  2  g  filter  experienced 
large  oscillations  after  the  maneuver  started,  the  cause  of  which  was 
not  found. 

The  multiple  model  filter  was  a  tenuous  alternative  at  best,  given 
the  data  contained  in  Table  2  and  Table  3.  Little  performance  improve¬ 
ment  would  have  been  gained  if  the  filter  would  have  worked  as  designed 
since  the  BM  filters,  used  in  the. bank  of  filters,  are  so  closely  tuned. 
This  performance  improvement  would  have  been  prohibitively  costly  in 
terms  of  computer  memory  and  run  time.  A  better  selection  of  filters 
for  the  multiple  model  algorithm  would  have  been  to  use  one  BM  filter 
and  one  CTR  filter.  A  wider  effective  dynamic  range,  over  which  the 
multiple  model  filter  could  operate,  would  be  gained  by  this  set  up.  This 
set  up  was  not  explored  since  it  wasn't  realized  until  late  in  the  re¬ 
search  and  the  additional  software  that  would  be  required  would  have 
needed  much  more  time  than  available. 

Another  consideration  in  actual  implementation  would  be  the  com- 
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Magnitude  of  Standard  Deviation  of  Error 
(time  averaged  actual/filter  estimated) 


e  3 .  Comparison  of  the  Standard  Deviations  of  the  Errors 


puter  resources  required.  As  nentloned  above,  the  multiple  model 
would  require  much  more  computation  time  and  computer  memory  for  the 
slight  improvement  in  performance  that  might  be  gained  If  the  filter 
worked.  The  CTP  filter  requires  approximately  the  same  computer  time 
and  memory  as  the  Bf!  filter.  The  savings  in  time  for  the  state  propa¬ 
gation  in  the  CTP  filter,  from  the  Taylor  series  approximation,  is 
offset  by  the  requirement  for  the  state  transition  matrix  to  be  updated 
each  sample  period.  The  Bf"  filter  requires  an  8x8  matrix  multiplication 
each  sample  period  for  it's  state  propagation. 

The  total  time  required  for  the  ?!onte  Carlo  simulation  of  the  3f! 
filter  was  about  one-tenth  of  one  percent  faster  than  that  of  the  CTP. 
filter.  This  was  unexpected  since  the  CTP  model  is  nonlinear  and  is 
due  to  the  various  approximations  used  in  propagating  the  filter.  The 
number  of  words  of  memory  required  './ere  exactly  the  sare  for  both  filters 
as  expected  since  both  the  ?"  and  rTP  filters  have  the  sane  size  and 
number  o*  matrices. 
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VI  ''onclusions  and  ^commendations 


Concl usions 

The  constant  turn  rate  filter  and  the  Cro./rian  motion  filter  are 
both  easily  able  to  track  the  three  trajectories  evaluated  for  this 
thesis,  '.,’hen  the  target  acceleration  profiles  are  not  demanding,  as 
in  the  case  for  the  2  g  pull-up,  the  performance  is  nearly  the  same 
for  both  filters.  !!ov/ever,  as  the  target  acceleration  profiles  become 
more  demanding  (10  g  and  20  g  pull-up  maneuvers)  the  performance  of 
the  CTR  filter  is  substantially  better  than  that  of  the  DV  filter. 

There  is  a  tuning  issue  that  requires  further  study  to  explore  the  re¬ 
duction  of  the  large  excursions  of  the  mean  error  outside  of  the  filter 
standard  deviation  experienced  during  the  periods  of  poorly  modeled  ac¬ 
tual  target  accelerations  in  both  filters.  The  computer  resources  re¬ 
quired  by  the  two  simple  extended  Kalman  filters  were  nearly  identical. 
The  total  time  required  for  the  Vonte  Carlo  simulation  of  the  Cfi  filter 
was  about  one- tenth  of  one  percent  faster  than  that  of  the  CTR  filter. 
The  number  of  words  of  memory  required  were  exactly  the  same  for  both 
filters. 

The  adaptive  multiple  model  filter  was  found  to  be  unsuitable  for 
use  in  this  environment.  This  was  due  to  the  RM  filters  being  so  close¬ 
ly  matched  in  tuning  that  the  residuals  of  one  filter  in  the  filter  bank 
were  not  significantly  different  from  the  residuals  of  the  other  filters 
in  the  filter  bank. 
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Reconnendati ons 


Both  the  Brownian  notion  and  the  constant  turn  rate  filters  need 
to  undergo  further  testing  of  their  robustness  to  variations  in  sig¬ 
nal  to  noise  ratio,  measurement  noise,  tuning  and  imperfect  initial 
conditions  to  provide  additional  insight  into  whether  or  not  the  CTR 
filter  is  truly  the  superior  all  purpose  tracker.  Slight  modifications 
to  the  BI:  filter  to  turn  it  into  a  first  order  Gauss-Markov  process, 
thus  making  the  acceleration  model  more  compatible  with  the  actual 
target  dynamics,  should  also  be  explored  and  compared  against  the 
CTR  model . 

Further  testing  of  the  adaptive  multiple  model  filter  also  needs  to 
be  accomplished  to  verify  the  results  of  this  report  and  to  test  the 
effects  of  having  different  filters  included  in  the  filter  bank.  A 
combination  of  the  Brownian  motion,  the  constant  turn  rate,  and  a  Gauss- 
Markov  filter  would  be  one  possibility.  Also,  update  rate  variations 
should  be  explored  to  study  the  effect  this  has  on  the  performance  of 
the  filter.  Another  area  that  needs  to  be  looked  into  is  the  possi¬ 
bility  of  using  only  the  center  4x4  array  of  measurements  and  residuals 
would  then  be  used  to  generate  an  8x8  A(t.j)  matrix.  This  should  re¬ 
duce  or  eliminate  the  scale  factor  employed  in  this  effort. 

Computer  Support 

The  pace  and  extent  of  this  research  effort  was  significantly 
retarded  by  the  computer  support  available  at  AFIT.  An  estimated 
30  percent  more  work  could  have  been  accomplished  with  a  more  re¬ 
liable  and  responsive  system.  The  author  encourages  any  steps  to 
make  AFIT's  computer  support  better.  Possible  recommendations  in- 
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elude  more  hardware  support  (l.e.  it  is  unbelievable  that  AFIT  has 
only  one  plotter),  more  personnel  support,  and  AFIT's  own  dedicated 
computer.  (Tef  3) 
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APPENDIX  A 


"F"  Matrix  Derivation 


The  calculation  of  the  linearized  F_  matrix  for  the  CTR  filter 
was  not  as  trivial  as  it  was  for  the  Brownian  motion  filter.  The 
dynamics  model  for  the  CTR  filter  was  nonlinear  and  therefore  the 
point  of  linearization  matters  and  it  cannot  be  precomputed.  The 
_F  matrix  needed  to  be  updated  every  sample  period  and  serves  as  a 
quasi-static  approximation  of  the  _f  (x)  over  the  sample  period.  Equa¬ 
tion  A-l  was  used  to  calculate  F_. 

9f(x]| 


8x  [x=^(ti ' ) 


(A-l) 


where  f (x)  is  as  previously  defined  in  equations  25  and  26. 
The  F  matrix  for  the  CTR  model  is  in  Equation  (A- 2). 


0 

0 

F1 

0 

0 

0 

0 

- 

0 

0 

0 

0 

F2 

0 

0 

0 

0 

0 

0 

0 

0 

F3 

0 

0 

0 

0 

0 

0 

0 

n 

F, 

0 

0 

0 

0 

Fs 

F6 

F7 

F8 

0 

0 

0 

0 

F9 

F10 

F11 

F12 

0 

0 

0 

0 

0 

0 

0 

0 

F13 

0 

0 

0 

0 

0 

0 

0 

0 

F!4 

where 

Fr 

F  =F  =F  =1 
*2  r3  4  1 

V 

-[2-x(3)(A1 

:A2’x(6) 

-  a 

2 

2"x(3) 

+  VA2 

2]/A13 
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F6=  -2*x(3)[-A1*A2*x(5)  -  A22-2-x(4)]/A13 
F?=  2-x(3)-x(4)-A2/A12 

F8=-2*x(3)2-A2/A12 

Fg=  -2*x(4)-[A1-A2.x(6)  -  A22-2-x(3)]/A13 
F10=-[2-x(4)*(-A1*A2-x(5)  -  A22-2*xC4))  +  Aj-A^J/A^ 
F  i  ^ = 2  •  A2  * x  C 4  )  2/ Ai 2 

Fl2=-2-A2'x(4)-x(3)/Ai2 

F13=  exp (At/ t) 

F  =  F 

^14  *13 

where 

Aj-x(3)2  +  x(4)2 
a2=x(3>x(6)  -  x(4)*x(5) 

At=l/30  seconds 

t=  correlation  time  of  atmospheric  jitter 
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APPENDIX  B 


Main  Program  and  Library  Listings 

This  appendix  contains  the  FORTRAN  IV  code  for  the  CTR  and  multi¬ 
ple  model  algorithms  presented  earlier  in  the  text.  The  program  list¬ 
ings  for  the  CM  algorithm  and  the  plotting  routine  can  be  found  in 
Reference  2,  Appendice  F  and  G.  The  GM  program  can  be  obtained  from 
the  multiple  model  program  by  deleting  two  filters  and  the  adaptive 
portions  of  the  program. 

Presented  first  is  the  CTR  program  listing,  followed  by  the  multi¬ 
ple  model  filter  algorithm  and  its  subroutine  listings.  Lastly,  the 
library  of  subroutines  common  to  all  three  main  programs  is  given.  One 
routine  not  shown  is  a  matrix  inversion  routine  from  the  IMSL  library 
which  is  available  at  AFIT. 
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Constant  Turn  Pate  Filter 


*A  F  THESIS  74/74  0PTS2 


FTM  4.8*528 


1 

C 

C 

t 


PROGRAM  THIS  IS (I NPUT  *Y  80 , OUTPUT, T AFifes 1NP UT , T APE60 J T OUT, T A » E c ) 
COMMDN/FLIF./  XFO^, YF  CV, I  MAX, NPIX, SIGHS, SIGHT, SIGMAS,  SI GFLR.RF 

*  ,  A  <?=>?.  0,  FI  MAX  ,U*i  (  2,  1)  ,  I  MEAS  ,  AR,  SluVF  ,  FL  2P  ( 6  4,  2)  ,  VM  A  X,  SIGMF£ 

*  *  R  A  N  3  E  C  ,  A  Nf:  E  ,  3  i  G  F  VF,C3TH,SKTH 
I  J  ”  r  3  :  ■  JNi 

REAL  IRAa 

DI  MENS  I  ON  Z  16*)  ,  PHI  (8,  6)  ,Q(3,3)  , 

*  WO=<( !, 3» , W(5),TEMr  (6 , t ) ,  1  £ MPl ( S, 6) , S AVE ( 14) , QFD 1 ( 3 , 5 ) , 

*10(8,6),  PHIF(3,0)  ,  QF  C(  fa,  8)  ,  SCQD(S»  6),XS(8)  ,H  (b*,»  3),aF»(8,6)  , 
*XrP(3)  ,FFm(«,c),HF  (64)  ,  EXTRA  (8,  fa)  ,  F.Ih  (  64, 1)  ,DXDXr  (8,  9)  ,PPFF(8,  £)  , 

*  H”  (5,  dM  ,  FZA(SL,r  J  ,CC5)  , R<6s, 64 ) , RN(64 , 64)  ,BD  (9,  2)  ,FHIFT  (8,6) 
•UFDMAx  (fi)  ,  H7ZH0(  2)  , DX { 8)  , SI G  (2 )  , 0  (  2)  , F FPOLD <  8,  3) ,  Xr*D(  8) 

* ,XFM(3 ) 

WRITE  (  6,  1)  . . . 

FORMAT (1H1) 


READ  AND  ECHO  DATA 


READ  *,  SIGSi 

PRINT  *,  **F "MS  DYNAMICS  FOR  TRUTH  MODEL,  SI3S1 

READ  *,  SIGMAb 

PRINT  *,  “KKS  KUYH  MODEL  BACKGROUND  NOISE,  SIGMAS 
READ  *,  S IGF  LR 

PRINT  *,  ■*RMS  TRUTH  MODEL  FLIR  NOISE,  SIGr.R 

READ  *,  IMfcX 

PRINT  *,  "TRUTH  MODEL  MAX  INTENSITY,  IMAX 

READ  *,  SIGAT 

PRINT  *,  "RMS  ATMOSPHERICS  FOR  TRUTH  MODEL,  SI3AT 

READ  *,  NR  UN 

PRINT  *,  "NUMBER  OF  MONTE  CARLO  RUNS,  ~  NRJN 

READ  *,  T FINAL 

PRINT  *,  "FINAL  TIME,  "  "  Tr INAL 

READ  *, SIGHS 


PRINT  *,  “INITIAL  RMS  TRUTH  MODEL  SIGMA  PERVEL,  SUMS 


READ  *,  ASTRO 

PRINT  *,  "TARGET  ASPECT  RAT  IC,  ASPRO 

READ  *,X0 

print  ♦,  "Initial  x  position,  .  <3 

READ  * , YO 

PRINT  *,  "INITIAL  Y  fOSITION, . .  Y3 

READ  *  ,  Z3 

PRINT  *,  "INITIAL  Z  POSITION,  -  -  -  -  ZO 

READ  *»  X  JOT  j 

PRINT  *,  “INITIAL  VELOCITY,  X  DIRECTION  ,  XDOT3 

READ  *,YJOTO 

PR INr  *,  “INITIAL  VELOCITY,  Y  DIRECTION  ,  YD3T3 

READ  * , Z^OTO 


", SIGS1 

“.SIGMAB 

",SIGFLR 

", I  MAX 

", SIGAT 

",NRUN 

",TFINAL 

",  SI GMS 

",  AS  PRO 

",X3 

“»Y8 

",ZC 

“,XDOTC 

", Y  DOT  C 
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ZDOrO  a  -,zdotc 


P=?INT  *,  "INITIAL  velocity*  z  direction  • 

READ  *  * ISPTL 

PRINT  ♦,  "SPATIAL  NOISE*  i-YES,  G-NO  IS^TL  a  ",ISFTL 

IF (I5PTL .NL. 1)  GO  TO  2 

READ  **C(l)tC(2>«Gm,CU)«C<5) 

PRINT  *,  "SPATIAL  NOISE  CORRELATION  COEFFICIENTS!  ** 

PRINT  »,c  (l)  ,C<2),C(  3)  ,C(h)  ,C<5> 

CON’INJE 
PRINT  ** 

READ  * ,  SIC  1FG 


PRINT  *,  "INITIAL 

FILTER  SIGMA  VELOCnV, 

SI GMr  3 

3 

",  S I C-PF  G 

READ  »,  tT.b 

PRINT  ♦,  "INITIAL 

FILTER  ASPECT  RATIO, 

ARG 

s 

",  A  R  C 

READ  *,  SIC-F2 

PRINT  *,  " RMS  FILTEF  ATMOSPHERIC  NOISE* 

S  I3r  2 

s 

",  SI GF  2 

READ  *,  RF 

PRINT  *,  "FILTER 

MEASUREMENT  NOISE  RMS, 

RF 

3 

”.RF 

READ  *,  SIoFiO 
PRINT  *,  "INITIAL 

RMS  DYNAMICS  FOR  FILTER, 

SI Grl 0 

s 

“,SIGF 15 

READ  *,  FIMAXO 
PRINT  *,  "FILTER 

MAX  INTENSITY, 

FIMAXO 

s 

".FIMAXO 

PARAM-TEh.  VALUES 


*************************************  ***********  ************* 

THE  HIGH  G  TURN?!  IS  PUT  IN  THIS  VERSION 

THE  HIGH  G  TURNN  IS  PUT  IN  THISVERSIGN -  - . 

************************************************************* 


XFOV«3. 

YFOV=8. 

NP IX»  8 
ATAUlslA.  1R 
ATAU2s  659.5 
FTAU2=1./A7AU1 
OT  a  1./3G. 

IREF*1 

FIMAXsAdS(FIMAXO) 
SIGV-s  ABSISIGHFC) 
SIGFl«Ab3(SlGF10) 


QFDMAX(l)  s.  2, 

OFOMAX  ( 2  i  =  OF  OMAX  C 1 J 
QCDMA*(3>  =  14. 

QFDMAX(4)=CFDMAX(3) 

QPDM»X(&)  =  20. 

Qc  DMA  X  (  6)  =  QFDMAX  { 5 ) 

Qr  0  M  A  X  ( 7  J  =  .5 

Ufdmax (8;uqfdmax(? ) 

iSilll^LIZL  TRUTH  MOJLL  V  AkIAc  Lib 

CALL  FANSE7( 7v6o2) 

ONE  =  1 
\'°S  =  6 

ms  =  np:x**2 

NFS=5 

NFS2=NrS  *NFS 
NrSN2=NFS-2 
NIS  =3 
RIsi./PF 

SNsSI3MAb/I;JAX 
IF (SN. LE. u. J  SN=.0C1 
SN= 1. / SN 

RANGE  0=SQR7(XC**2fYt  **2*Zu**2) 

AGAIN  s  .361006534  *  SIGAT 

IFILE=5 

CELT  ?  -l.*JT 

00  5  1=1,6 

30(I,I)  =  3. 

BD(I,2)=L. 

00  5  J*l,6 

QO  ( I »  J )  =  0.  -  -  - 

SOQDd, J)  r  0. 

PHltl,  J)  s  3. 

CONTINUE 

FACTs(  AGAIN*  «2)  *  (  AT  A  Ul  **2>  M  ATAU2**4>  ~ 
F4CT«( AGAIN**2)*(ATAU1**2) MATAU2**4) 
FACTt  =  A7AU1-ATAU2 
FACT?  *  A7AUH-ATAU2 

FACT3  =  2. *ATAU2  -  -  ~ 

G1  =  FACT/(FACT1**4) 

G2  =  -  ACT/(FACT1**3I 
G3  *  F  ACT  /  (FACT  1**2) 

R1  *  1.-  EXP (  2.  *AT  A  U1*DELT  ) 

R2  «  1.-  EXP  (FACT  2*DELT) 

R3  =  1.-  LXP(2.*ATAU2*DELT> 

R4  =  3T*iXP(DELT*FACT2) 

R5  =  DT*EXF(2.*ATAU2*iiELT) 

FILL  OUT  TF;UT  H .  MOOtL  PHI  MATRIX.  -  • 
SEE  MERCIER *S  THESIS  FOR  DERIVATION 


o  o  n  o 


°HI(1, 1) 

-  1  • 

PH  I  ( 2,  2) 

=•  PHI  (1,1) 

PHI(3,  3) 

=  EXP(ATAU1*DELT) 

PH  I ( 4, 4) 

=  EXP  (A  TAU2  #DELT) 

PHI (♦, 5) 

=  DELT*PrtI ( 4, 4) 

PHI (5, 5) 

=  PHI(*t,4) 

PHI (3, 6) 

=  PHI (3, 3) 

PH  I (7, 7) 

=  PHI  (4,4) 

®H I ( 7  ,  t ) 

=  PHI  (•*,  3  ) 

PHI  ( 3 *  F  ) 

:  P  hi (p,5> 

WRITE( £  , 

11) 

11  FORMAT  (///2X,”THE  7  r.  IT  H  MODEL  S':  lit  *  KANSIT ION  MATRIX  IS***/) 
C 

FIlL  DU*:  DISCRETE  INPUT  MATRIX 
C 

33(1*1)=  07 
bD(2,2)»-  u ’ 

WRIT£ ( 5 , 15  ) 

IS  FOP.MAT  (///  2X*  "THE  TRUTH  MODEL  INPUT  MATRIX  IS*"/) 

FILL  THE  QD  MATRIX  WITH  VALUES  USING  EXACT  INTEGRATION 
SEE  HERCIER'S  THESIS  FOR  DERIVATION  - 

QD(1,1)=  SXGS1 
QD  (2»  2  )  -  QD  (1,1) 

QD  (3, 3  )  -  (G1*R1  )/(2.*ATAUl) 

QD(3,J»)  =  R2*  (G2/rAC  72**  2 -G  1/FACT  2) -R4*G 2/F  ACT2 
QD  ( 3 ,  5 )  -  G2*R2/FACT2 
Q3  ( 4, 3 )  -  QQ(3,4) 

QQ  ( 4, * )  =•  f.3*(Gl/FACT3-2.*G2/FACT  3**2»2.*G3/FACT3**3) - 
•  R5MS2/  ATA<J2«-G3*DT/  FACT  3-2 . *G3/FAC73**2) 

QD(4,5)  =  F 3*(G3/FACT3**2-G2/FACT3) -RE*G3/FACT3 

00(5,3)  ~  Q  J  (3,5)  . - . -  - 

QO ( 5 , 4 )  =  Q3(4*5) 

QD  (5,  5 )  -  R3*G3/F  ACT  3 
DO  20  1  =  3,5 
00  20  J=3, 5 
QD (1*3 , J+3)  =  QD (I , J  ) 

Q(I-2,  J-2)  =  QD(I,  J)  - 
20  CONTINUE 

WRITE  (  6*  30 )  . . . 

30  FORMAT (///2X,"THE  TRUTH  MODEL  QC  MATRIX  IS*"/) 

C  -  -  •  - 

C  TAKING  GH0LE3KY  SQUARE  KCCT  OF  QD 

C  -  .  -- . —  . . 

SQQDd,  1)  «•  SQRT(QO(  1,  1)  ) 

SQQD( 2,2)  =  SQQD(lfl) 

CALL  :HOLESK(Q,WDRK,  NIS) 

DO  33  1  =  1,  MS 
DO  33  J=l,  NIS 
SQQ0( I +2, J+2)  s  WORK  (J,I) 

SQQD( I *5,  J*5)  =  WORK  (J, I) 

33  CONTINUE  -  - 
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c 

L 


36 


HR ITE(  bt3«. ) 

35  FORMAT  l///2X»**7H£  CHCLESKY 
CALL  MOUT (SQQO,NPS,NPS) 

Ip  C 1 5  PT u  «  N:]  «  1 )  GO  TO  *,1 


SGUAKE  ROOT  OF  QO  IS***/) 


SET  UP  SPATIAL  NOISE  CORRELATION  COEFFICIENT  MATRIX 


N=  6  A 
Ms  9 

JO  35  hi,l. 

ItD  *1. 

I c  Tl^ct^o1!  GO  TO  3b 
°.C»I  +  ll-C(i) 
l"  (I.  :•£.  o3)  C-C  To  3  b 
Rl  I,  1+  2)  =C  (3) 

Ip  (I.GE.57)  GO  TO  36 
R(  I » I*  5)  =C  ( -*) 
R<!«I+7)-t (21 
R( I, !♦ 8) =C(1J 
PC  It  I*  9) -0(2) 

R< I»I*1G)=C(4) 

IF  (I.3E.45)  GO  TO  36 
*( 1,1* 14) =C( 5) 
R(I,I*i5)*G(4) 

R( I»I«-lb)  =  L(3) 

R( I» I* 17)  =C<4) 

R(  I,I*18)sC<5) 

CONTINUE 
□0  37  Is 1 1 


R{6*I-7,a*II=0.Q 
R(  3*w,  a*i-u  *o.c 
R< 8*1-5, 6*I)=0.Q 
IF  TI.5E.8)  GO  TO  37 
R(  8*1,  8*1  +  11  so. u  - 
Rl  8*1,  e*I*2)  =0.0 
R<  8*1-1,  6*I+l)s»0.t 
*18*1-7,  3*1*7)  =C  .0 
R(  8*1-7, d*I*8)«0.0 
R(  3*1-5, a  +  l  +  d)  *0.0 
IF  U.3E.7)  GO  TO  37 
Rt  3*1,  8*1+9)  =u  .0  " 

R( 8*1,  8* 1+10)*>G. 0 
R(  8*1-1, 8*I*S)*0,0 
IF  (I.3E.6)  GO  TO  37 
R(  8*1,  8*I  +  17)  =  0.  0 
R( 5*1,  5*1* 1C ) =0, 0 
Rl  8*1-1,  ii*I*17  )sQ.  0 
37  CONTINUE 

00  33  Ib1,N 
L=I«-1 

00  38  J*l,N 
R(  j, I) *k  (I,  Ji 
36  CONTINUE 

00  39  1*1, N 


52 


o  o  n  r*  nr  o  r  n  r,  n  o  o  .»  j  r  nr-  r 

Ml  V'  w  u  H  i 


33  79  J*  1,  t. 

ON  (I,  J)  =  3Ib  1A..  *r.  ( 1 1  J  ) 

29  CONTINUE 

WRIT£<  5,  *.,) 

v*-  FORMS?  (///2X»"b4  X  La  SPATIAL  CORRELATION  MATRIX*-/) 

com°u‘i.  ~he  choll sky  square:  root  of  rn 

CALL  3  MO'-EE  *<(f  .'•»  K*  o*»  ) 

WRIT£(  £,  -,ci 

F0R“1T  <///2>- Cm  CL  3S KY  SQUARE  ROOT  OF  hN*"/> 
o3  TD  -2 
DO  *3  I— If 
F.(Ifl)  si. 

CONTINUE 

S£T  UR  FILTER  MATRICES. 

00  51  1= If  NFS 
00  51  J=1 • NFS 
PHIF(I, JisC. 

1  QF0(I,J)-G. 

FI.L  UU'i  FILTER  F HI  MATRIX 


IT  IS  FROM  HERE  ThAT  THE  ORIGINAL  PhlF  ELEMENTS  HMERE  REMOV 


00  55  I-lfK-S 
DO  55  J=l,  NFS 
6  •  PHIFTTI, JJ^PHIFtJ, I) 


CILL  OUT  FILTER  DISCRETE  OFD  MATRIX 
FOR  START  OF  ACQUISITION  PHASE 

QF 0  ( 5 1  5)  -SI'3F1*DT 
QFO(S,  6)- QF3  (E  ,5) 

QFO  (7,7)  =  (SIGF2**2)  *d E XP (  2  .* OELT/F TAU2J  ) 

QF 0  (6,6)^  GFD  C7t  7) 

HRIT£(5»hO) 

h3  FORMAT {///2X, "THE  FILTER  STATE  TRANSITION  MATRIX  IS*"/) 
CALL  MOUTTPHIF, NFS* NFS) 

WRITE ( 6«  *5) 

h5  FORMAT {///2X, "THE  INITIAL  FILTER  QD  MATRIX  IS**V) 

PRINT  «V*  " 

PRINT  "  . . .  .  . 

C 

PRINT  eEGIN  THE  MONTE  CARLO  SIMULATION  - 

C 

DO  99  L* If  NRUN  •  - 

TIME  »  0. 

XCENTR  *0  •  ...  -  ----- 

YC  ENT  R  =  0 . 
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o  o  c  o  o  r>  o 


FI  ’ll  V  *ti. 

IMEAJ=  C 

FIMAX*  fibstFlMAXii) 

AR=AR2 

SI GVr*  Abi  (ijIGMFu  ) 

SIG^ls  AuS(SIGFlC) 

VARY Qt  Abo (SIGF10J 
TRXXT=2j:. 

MANIN3  =  t 

i 

i 

p;i  :  i  i.n.1  * *« w  .  Jy,  -I  tiC.,L  F GR  i« c. W  r u  1*  ! 

03  45  I=i,i.:S  I 

XS  (I)  =  j. 

-»6  C0NTINJL 

30  4 7  I  =  i,  NFS 
XFP(I)  a  .. 

XF  M  C I )  =  o  « 

DO  47  J=1 ,  NFS 
QFD(I,J)-D. 

PFP(I,  J)  *  ).  -  - . 

47  CONTINUE 

XRES1  =  u  .  -  - 

XRES2  =  £-. 

XRES3  =  0. 

XRES4  = 

- XRES5  *  vi. 

XRES5  =  y. 

XRES5  =  0. 

PROVIDE  FILTER  WITH  INITIAL  VELOCITY  AND  POSITION 

RHOR*( X0**2*ZQ**2>  -  . 

RANGE=  <RriO«+YC**2) 

XFP(3) =<Zu*XDGTO-XO*  ZOOTO)/ (RHCF*. GCuC2» 

RH0R=S  QRT (RHOR) 

XFPU)  =  <RHOR*Y  COTO-YO*  C  ( XO*  XCOT  C *Z0 *ZCOT  0) /RHORH  /  (RANGE*.  ODCO. 
PRINT  *,**  F.JN  NUHL-ER  ",NRUN  . 

PRINT  *,**  RHOR=“tRHGK*"  RA NGE«“, RANGE XFP (3)  ■  <FP  (  3)  ,  i 
*"  Xro  (4i  =**i  XFP14)  - 


•THESE  ARE  THE  DYNAMIC  MODEL  CHANGES  MADE 

Ql=XrP  (1) 

<iZ=X~3  (2) 

Q3*XrP(3) 

Q  4*X-P<4> 

04 = X -  3  (4 i 
Q5=X*P<5) 

Q5=X-3  (b) 

0T=Xr»<7) 

Q5  =  X-3  (6) 
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PErFO-.N  TRU7rt  MOL-  tL  SIMULATION 

YTIME*TIME-3T/2. 

IF  (TIME-J.  c)  5j3,:0£,505 
500  XDOT.-1CJC. 
rooiss . 

ZOGT  *  0  • 

X  )EH=<  D+X3CT  *7  I  Me 
Y7£H=YC 
Z7-  H=Z  C 
r3 c  r:  sic 


u 


L 


<■*  *r  t  T  c* 

V-  :  r  1 5 


0 

0 

C 


510 


C 

C 


.  I  i2,  i 

IS  A  2  &  TUN 

SS'-JFtSii  !  ;•  j  :.SE 


sssrm  ? 

S»iS»liiiStHSii-i>3i3SJ$£5  5SS}£S$i 


XDOT«-10JC.*COS(.019b* (VTIME-2.)) 

YOOTsl 00 j.*SIN(. 0l9b*( VTIMl-2.)  )-  - 
XVSH«X C-2G 03. -51020. *S IN (.0196*  (TIME-2.) ) 

YV£H  =  Y9+r  li.2G.*(l. -CCS  (.0196*(  TIME-2.)  )) 

ZVEHxZO 

CONTINUE 

RH0R«(XVEH**2*ZV£H**2) 

RANGE*  (RHOF.+YVEh**2) 

UT  (1,  l)  =  (ZVEH*XuOT-XVcH*ZDO.T)/(PhGR*.LCGC2) 

RHOk  =  S  QRT(  kriOk) 

UT (2)1)— (kHGR*YOOT -Y VEH* ( ( X  VEH*XCGT  «-ZV  EH*  ZCOT)  /RM OR)  ) / (RANGE 
*.  0-330  2) 

RANGE* SORT  (RANGE) 

VMAXsSQk  F(X30T**2+YDOT**2-*-ZC0T**2)/IRA N3E*. 03002) 
XCENTRsXLEMR+LT  (1,1)*BC(1,  1) 

YCENfRsYCENTR+UT <2, 1>*6D(2, 2) 

CALL  NOISE  (NPS,W) 

CALL  MMPY (TEMP,SQQD,  W,  NPS»NPS»  ONE) 

CALL  MMPY (TEMPI, PHI, XS  ,NPS, NPS, ONE) 

CALL  MAOu(XS,T  EMP, TEMPI,  NPS  , ONE,  ONE) 

CALL  MMPY  (T..MP1,  3D,UT*NPS,2,  ONE) 

CALL  MAD0>(XG,XS,  TEMPI,  NPS, ONE, ONE)  - 

CALL  SHIFTACXFP,  XF  PO  ,  NFS  ,  NF  S ) 

FILTER  STATE  PROPAGATION. 


OMEG*(Q3*Q6-Q4*05)/Q10 
XFM(1)*XFM1)«-Q3*DT 
XFM(2)sXFP(2)  ♦Oi+*OT 
XFM(3)«XFP(3)*Q5*DT 
XFM(4)=XFF(  h)  ♦Q6*0T 
XFM(5)  sXFf  (•3)-M-0MLG**2)*03*QT 
XFM(5)cXFP(b)^(-OME  G**2>  *Q4*3T 
XFM(7)  sXFP(7)-(Q7/FT  )U2)*DT 
XF  M  ( 3 )  =XFF  (ci)-(Q3/FT  AU2)*DT 
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00000*00  o  o  o  C 1 


C  FORM  CENTROID  POSITION  A  NC  FILL  TRUTH  ARRAY 

C 

X»EA<  =  XS(l)  ♦  XS  ( 3 )  ♦  XS ( A )  -XFK(l) 

Y°£A<  s  XS12)  ♦  XS(fc)  ♦  XS  <  7 )  -XFM2) 

Ie  (A3S (XP^A K) .bT.7.*ASPR0*SIGMS)  GOTO  101 
IF  (A3S  ( Y  Di_  A  < )  •  GT  •  3  •  *  *-  S  PF  0*  3  I G  MS )  GC  TC  1C  2 
CALL  MEA  OVEA<,YT  tA*,  n,Z,r.) 

Ic  t-I  w.  :  .  .  - .  J)  jL  T  C  1C  C  2 

PRINT  * ,  ”  “ 

°R I NT  *,“Z" 

CALL  1  OU  '  i  Z,  NT  IX,  NP1  X) 

11  0  2  CO  NTI N'  UE 

W 

L  SEARCH  FOR  FI  MAX  AND  FI, TIN 

C  SEARCH  FOR  FI  HA  X  AN  0  FlMlN 

L 

IF (FIHAX J.GT.O.)  GU  TO  59 

F I MXO=  F I  MAX  . - 

FlMAXiO. 

FI  MIN=0. 

□0  55  1  =  1, MS 

IF  <ZtI>  .  TT.FIMmX)  FIT'AX  =  Z  C  I> 
as  IF  CZ ( I  )  .  LT  .fIMIN)  Fi  Ml  N  =  Z  (I) 

FI  MAX  =  FI  ••  A  X*.1h*SI&VF**2-1  •  52*SIGVF  +4  •  35 
FI  MAX*  .o^FlMXO*  •  2*  FI  MAX 
55  CONTINUE 

FIL1E,;  COVARIANCE  PROPAGATION 

CALL  3HIFTAtPFP,PFP0LD,NFS2,NFS2>  ~ 

CALL  MMPY (EXTRA, PH  IF  ,PFP,NF  S ,NFS, NFS) 

CALL  M  MPY  < FP  FF , EXT  RA ,PHIFT , NFS, NFS , NFS ) 
o?FFc  PHI  *  PIT  I-D+  *  PHIT 
CALL  MAOJ(PFM,PPFP,QFO,NFS,  NFS, ONE) 

P-M  =  P  (TI-)  =.PHI  *  F  (T  I- 1 )  ♦  *  PHIT  ♦  G  *  Q  *  ST 

PERFORM  MEASUREMENT  UPDATE  FOR  THE  FILTER 
I NV  £F;SE  COVARIANCE  FORM 

FORM  FILTER  CENTROID  POSITION  AND  FILL  OUT  NON  LINEAR 
SMALL  H.  CALCULATE  PARTIAL  SMALL  H  PARTIAL  X 

XPEA<  =  XFW7)  . - 

YPEAK  *  XFMC  8) 

IF  {  (X-  M(  3)  ♦*2«-XFH(h)  **2)  .EQ.O.)  XFM(3)s ,001 
CALL  ME  ASF  (XPEAK, YPE  AK, ONE , HF, H, XF  K) 
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on  n 


JD  63  I*-  1,  N  -s 
DO  6D  J=I,N-S 

HT(J,  I) 

63  CONTINUE 

CALL  *MPY <PFP,HT ,h, NFS, NMS, NFS) 

l  hi  *  k  (=-i  is  scal*p  anc  multiplied  la r ER) 

IDGT  *  c 

CALL  uIM/Ef  <rFH,  JFS,  NFS, EXT  ~A,ILGT  ,  HKA  F  E A » I£R) 

v  EXT'"' A  r  ( “I  -)  -1 

DO  5;  i=lj  ?  *  G 
1_0  >5  J-  1,  :.;‘w 

c-  r  (I,  J)  -  f-F- c ,  J)  **1  ♦  l  XT  f-  m  (I ,  J) 
t5  CO *IT I N  JE 

L  =r3=  P(7  •.«•)-!  =  nT  *  R-l  *  H  +  F(TI-)-l 

I J  GT  =  G 

CALL  L  IN  .'2F{  PFF,  NFS,  liF  S,  EXT  RA,  ILGT  ,  WKAF  EA ,  I ER) 

JO  73  1-1,  ;."s 
DO  7  2  J=  1,  NF  S 
PFP (I, J)  =  EXT kA ( I , J  ) 

70  CONTINUE  -  -  -  - . . 

00  130  1-1,  NFS 

IF  (P-3  (I,  n.GT.O.)  SO  TO  130 
PRINT  *,**PFP  r,I,I,“  )-**» PFF  (1,1) 
pFP(I,I)-PFPOLD(I,I) 

130  CON'INUE 
C  Pr 3  =P ( 7I+) 

CALL  M  O'J  (f  IH,Z»Hr,NMS,ONE,NFS) 

L  RH*k:SI  DUALS*  Z-SMALL  H 

IF  (SI3MF  j.C-T.C.)  SO  70  62 

COMPUTE  -JEW  ESTIMATE  OF  SIGVF  AND  A k 

“0  53  1=1, NMS 

AR=  AR  +  .  jjl*  (RIH(I)*FL2P(I,  1)) 

SI GVr  =  SI SVF*. 2  01* ( RIH ( I)  *PL2F(1 ,2)  ) 

56  CONTINUE 

IF(A?.Lt.l.)  A p.=  1  • 

IF  (SIStfF.LS.  0.)  SI GVF=  SIGMF 0  - 

62  CONTINUE 

CALL  M  MP  Y (EXTRA, HI »  F IH » NFS »  NMS, GNE ) 

C  E<T5.A=  H7  *  (Z-SMALL  H) 

CALL  MMPY(DX,PFP, EXTRA, NFS, NFS, ONE)-—  - 
C  OX  =  P(TI+I  *  KT  *  (Z  -  SMALL  H) 

00  75  I*  If  NFS 
OX  ( I ) =  DX  ( I ) *RI 
75  CONTINUE 

C  DX  =DELTA  X  =  MTI+)  *  HT  *  R-l  *  (Z  -  SMALL  •(> 

DO  78  1=1,  NFS 
DO  73  J=1 , NFS 
OX  QXT ( I , J)  =  OX  (I)*OX  (J) 

7b  CONTINUE 

IF(TIME  .LT,  1.95)  GU  TO  1JGE 
1005  CONTINUE 
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o*:xi  t  ~  i ♦  >  -  x ( t i -) )  *  txni*)  *  xdi-ni 

TR XX CO=T  <xx ; 

T^xxr* : . 

DO  79  Is  1, NFS 


t  'i 


791 


lit 


11*. 


139 
1  *♦  G 


112 


C 


TRXXT=T(<XXT«-QXDXT(I, 1) 

THRESH  -  AI-.S  (EXTRA  (1,1))  ♦  AES  (t  XT  PA  ( 2 , 1)  ) 

IF  (THRESH. Li. 2120.)  GO  TC  112 
IP  (MSNIItw.L'J.l)  GO  TO  112 
~C C=  S 0 FT  (  1 .-  (l./AE**  i)  ) 

3 1  ^  ( 1 )  :  5  Ik?  :: IGPV/F**?/  (l.-C  *:CC*GL’.  H  )**2H 
SI  <!-'  2)  -  i  );.  I  (  SiuPVF  **  c/  (1 .  -  (  £CC*bf-  •  r)  **i)  ) 

PPINT  *,**  iIG(l)  i  1G  (1)  SIC-  12)- •**  SIG(2)  »**wCSB 
DO  lit  1-1*2 
OH  T  Z*E XT  .A  (I) 

AL 13  0  =  ( A- OGi 0  (AoCtuhT  Z) )  ♦  .  A  651*SI  L-  ( I ) -3. 9)  /  .  b2 
□(I)sEXP(2.333*ALi:C) 

AOH'Z  c  Abi(  3  7Z) 

IF  (ADH7Z.GT.10U.)  GU  TO  791 
3(1)  *  0. 

CONTINUE 

PRINT  (“»I,“)-M*  CCI) ,“  DHTZs  ".CHTZ 

XFp(I«-4)  =  <  2.*D(I)  /  OT  **2)  ♦DHTZ/AfeS(DHTZ>*XFPO(I*4> 

CON“INUE 

DO  11 A  I- 1* NFS 

IF  (I. EG. ?)  GO  TO  114 

IF  (I.EQ.b)  GO  TO  114 

XFP(I) =X  FPO (I) 

CONTINUE 

CALL  SHlFTA(PFP0LG,FFPtNFS2*NFS2) 

00  14C 

VF ACTDRs iC./SQRT (PFP (1,1)  ) 

AF ACT 0 Rs3 0 C. /SORT  (PFF(  1*2,1  *2)  ) 

DO  139  J=1 ,  NFS 

PFP(I,  J)sPFP(I,  J)»VFACTOR  .  '  -  ” 

PFP(J, I)=PFP <J,I) *VFACTOR 

PFP(I  +  2,JJS:0FP(I*2»J)*AFACTCR 

PFP(J, I*2)=PFP(J,I*2)*AFACT CR 

CONTINUE 

CONTINUE 

MA  NIND=1 

GO  TO  121 

CONTINUE 

MANlNDsO 

TRXXr=.3*TRXXTC*.2*TRXXT 

CALL  M  ADD  ( XFP, DX,XFM, NFS, ONE, ONE) 

Xr3=X(TI*)=  P(TI*)  *  R-l  *  Hi  *  (Z  -  SMALL  HI 
IF  ( TI ME  .L7 • 0. Oh )  GO  TO  202 
XRES1  =  XFP(l)  -  XFM(l) 

XRES2  s  XFP (2)  -  XFM ( 2 ) 

XRES3  =  XFP(3)  -  XFM  (3 ) 

XRES4  *  XFP<4)  -  XFM  (4) 

XRES5  =  XF  F(  5)  -  XFM(5> 

XRESS  *  XFF  ( 6)  -  XFM  (6) 


<  (TI- 
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VI -’1  =  Aril /.\L  ill 
AX-2  =  ALSIXF.ES2) 

IF  UXR1.G7  .ii.5»  30  10  2C1 
XR  ESI  r  u. 

XRES3  =  u. 

XRES5  *-0. 

2  j 1  CONTINUE 

IP  l  AXF2.G'..,'»)  Si  TO  232 

X  "  3  Z  r  ). 

X  .  v  -  5  .  • 

X  E  S  a  r  « 

212  CVri'i'JE 

I F  ( 7 1 M  r  •i.T,  l.'di))  3  0  ~0  1j  l>6 

l.Cc  CO NT  I  HUE 

C  THI^  IS  ”r-L  NiW  jY NAM ICS  “.GLEL  IN  IHl  TIKE  LOGP 

c 

qi=x;=>  ai 

Q2=XPP (2) 

Q3  =Xro ( 3 ) 
q 4=XFP (4) 
q=;=xP3(5) 

Q6-XPP  <o> 

Q7=XP= (7J 
Q3=XP3 (fl) 

Q1C  =  33**2+Q>.**2 
PH  IF  ( 1  »  1)  ~  1 
?HIF(2, 2i  =1 
PH  IF ( 2 , 2 ) =  1 
PHIF(3,3)=1 
PH  IF  ( 4 , 4)  =  1 
PHIF<  1 , 3)  =l>7 
PH  IF (2  *  4) -LT 

PHIF(3,5)=OT  -  -- 

PHIF(t,6l=L7 

A1SQ=A1**2 

A1CU3E  «Al**3 

A2SQ=A2**2 

PHIF(5»‘3)--(2*Q3*(A12*Q6-A2SQ¥2*Q3)  +A2SQ*A1 > *DT7 All J 3E 

PH  IF (5  ,h)  =  -2*03M-A12*Q5-A2SG*2*Q4)*DT/A1CUBE 

PHIF(5,5l  =  H-2*Q3*Q4*  A2*DT/A1SQ 

PHIF(5,6)=-2*Q3**2»A2*0T/A1SC 

PHIF(6i  3)s--2*Q4»01*(  A1Z*Q6- A2SQ*2*Q3) /AiCUBE 

PH  IF <5, 4 -  =  -<2*Q4*( -A 12*Q5-A2SQ*c*Q4)+A2SQ*Al)*DT7 All  J9E 

PH  IF  (6, 5i-2*A2*Q4**2*DT/AlSC 

PHIF(3»6)'l— 2^A2*Q*t*G3  *DT  /A  1SQ 

PH  IF  (7, 7  J=LXP(0ELT/FTAU2) 

PHIF(fi,8)t.FHlF(7,7l 
00  5311  J=l»  NFS 
DO  5011  1=1,  NFS 
pull  PHIFT(  I,  J)=-CHIF(  J,  I) 

OMEGA* <Q3*QA-Q**Qp)/Q13**.5 
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o  r>  n  o  ■  o  n  n 


a:qui  i*  ion  ok  ^tu-ation  of  qfd 

IF  (SI3Fl*,bT.C.)  GO  FO  7L 
CALL  SHIFT;  CQ FL,  QFL1»  NFS2,NFS2) 

Ic  (TIh'£-.l-)  7  2,  72  ,  i  7 
57  Ir (TI P£- . 5  )  61,63, tC 
el  IF  (T^XKT-lt- J.)  72,7  7,77 


«/A»0=t/  A'  Yy-ilC-Fl*,.  £.,44~4hh4 
Q- -Ml,  1)  _,7  **£  *,/«•  vn/2u. 

QFQ(1,  3)  -L  *.  ****tf„.-.YQ/3. 

Is  0(1,  5)  ^37**3*tfARyQ/£. 

QrD(2, 2)-  QF3 (1, 1) 

Qr  D  (  2 ,  A)  -  QFJ ( 1 , 3) 

Qe  D  ( 2  »  6) =  QFj  (1,5) 

QFD(3»l)=QFj(l,3) 

QFD  (3,  3)  :  DT**3*tf*RYQ/3. 

GF0<3,  5)--0T**2*tfA*YQ/2. 

QFD(4,  2) =QF 2(3,1} 

QFDU,  L)  =  QFJ  (3,3) 

QF  D  (  4 ,  4)  -QF  j  (3,3) 

QF0(4,fc)=QFJ(3,5) 

053(5,  1)* QFJ (1,5) 

Qc  0  ( 5,  3)r-QFj(3,6) 

QF  Q  ( 5 ,  5)  VaFYQ*3T 
QF  0  ( 5,  2 )  =  QF-J  (1 , 5) 

QF  D ( 5 ,  4) -QFj  (3,5) 

Qr0(6,  6)  ~QF0(5,5) 

Qr  0  (7,7)  =  (SIGF2**2)  *( 1. - EXP ( 2. *DELl /FT AU2 ) ) 

QF  D  (P,6)=QFD  (7,7)  . 

GO  T 3  74 


ESTIMATION  OF  QFD 


77  PRINT  *,  **  •* 

PR INT  t OAPTI ON  STARTED  AT  “.TIME,"  SEC." 

6!-  CALL  HA03(QFQ,  3XDXT  ,  PFP,  NFS  ,  NFS, ONE) 

CALL  MADDtCFD,  GF  3,  PP  FP,  NFS  ,  NFS,-1) 

Q?D=  jXDXT  ♦  P(TI+)  -  PHI  *  P(  T 1-1 )  ♦  *  PHIT 

BOUNDING  QFD 

DO  84  1*1, NFS 
QF  ACT0R=1. 

Ic(Qr3(I,I).GT.O.)  50  TO  86 
QeACTOR=:j. 

QF D ( I ,  I )  *  3.1 
GO  70  85 
66  CONTINUE 
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:r  <  iFj(i,  in,  . 0? Duty,  tin  cr vl.opsQflmax{I>  /so-hqfcu,  1) 

1^  (^*^C  )  oC  TO  6  4 

QFDtl,  I)=QF,-CT0R**2*QF0<I,n 
6:  CONTINUE 

DO  32  J=i,NFS 
IF  II.EQ.  J)  GO  TO  62 
Qc  D  ( I  »  J)  =  QFJ(ItJ)  *QF  ACTOR 
e?  CONTINJE 

00  43  Ksi,NFS 
IF  (I.EO.K)  -0  VO  63 
Q-0(<,  I)=0Fj(K,I)  *QF-LTCk 
C'J'.’INUL 
•>  •  J  jN"I  N  Ul 

JO  "*3  I:  If  f>‘  j 
00  7?  J=l,f,TS 

73  !JCDII,J):  ,2*QFj  (I,  J  )«■  .e*QFCl(I,  J) 
i-  CONTINUE 

C 

L 

C 

C  WRITE  DATA  TO  FILE  TAPES  -  - 

C 

SAVE<1>  =  XS(1)  . 

SA VET  2 )  UT (1*1) 

SAVE!?)  =•  XS  ( 2) 

SAVE(4>  -  UT ( 2  *  1 ) 

SAVEC5)  =  XFPC1J 
SA  VE ( 6 )  -  XFP ( 3) 

S A  %/E  (  7  >  =  XFP  <  2) 

SA  VE ( 6  )  -  XFP(4) 

SAVET9)  =  XCENTR 
SA  VE ( 1 G J  s-  YCLNTR 
SAVE(ll)  =  °FP  (1, 1) 

SA  VE  ( 1  2)  =  PFP(3,3) 

SAVEC1  3)  =  PFF  (2,21  -  •  -  - 

SAVEC14)  =  PFP(t»4) 

WRITE  (  5)  SAVE  •  - - -  .  - 

SO  TO  50 

1.1  PRINT  *,  "LOST  TRACK,  X  CHANNEt,  MEAS  CALLED  **,IiEAS,  **  TIMES. 

*  RUN  ”,L 

PRINT  TIME  *  ”,  TIME 

GO  TO  105 

132  PRINT  *,  "LOST  TRACK,  Y  CHANNEL,  MEAS  CALLED  "fIMEAS,”  TIMES. 

*  RUN  ”,  L 

PRINT  TIME  «  ",  TIME 

1-5  DO  110  J=i,l * 

11k  SAV£( J ) *  0. 
l.fe  WR ITE ( S)  St*  VE 
TI  M£»  T  IHE+DT 

ICTTIME.GT.TFINALJ  GO  TO  99 
GO  TO  106  • 

99  CONTINUE 

ST 0°  "FINISH” 

ENO 
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t 


Multiple  Model  Adaptive  Filter 

7,  / ,  .  :■>'=?. 


ppgr,P£f!  t-'.  rrs  i%p  u'  =  /  out?  jt,  ~ap-  s  =  iuput  lp-  o=qutput  ?  .  - _ 

COMMON/7!.:^/  XFjV,Yf3v,IMA>  ,  N3  I  X  ,  S  IN  MS  ,  S  It -MF ,  SIo  *  AC  »  SIw  F~LR,  RF 

*  tA.2PRQj.Fl  ti.*  X  i  UT  1  El  2P.Lt-.iu  2)  .jlUMtZL-  HE  C _ 

**rangec*  ^  i  \  r-  =: ,  si:*pvf,  csn  ,swn 

T£GLR  0N: 


♦  N9RK(3,  3)  .  L(  £5  ,  TEhF  <£.  ft)  ,  T  £:  M  =»  1  (  ?,t>  .  SA  J~  (  1*  J  ,  CF  01 C  8 ,  6 )  . 

*  'll;  ft ,  ft )  i.b!  -  Fie-  «  ). .  0  r  .  (  f : .  5  )  .  S  23 1  Jcj  c  )  .  XS  (  ft)  .  H  (6».«  3)  « PFP  E )  , 

*>  FP(6)  ,3F,*\  i,  9)  ,  -.F;  t i. )  , ' X  IS.  (3  ,  9  )  ,RIh  (  l)  t  CXDXT  (  5,  f  )  ,  PPF-  f  3,  )  , 
W*U-li511F..«: .<  LJ  jC(  F )  ,  RQ  L_.  6- U.F M6 1 16-*)  .r  j .( <i,  ? )  ^ IF'  (  E .  81 
*nT7H0(2l  ,  OX  l  5  )  S I C  (  ?  J  ,  0  i 2  )  .  PF3  C  L  D  (  E,  ft )  •  XPPO  (  8 )  ,XFP1  (  9  )  - 
♦XFP2(S).  Xrs3t  &;  .  <F-  SI  (  £  ) ,  XFIS?  ■  6  >  .>  P;  S3<  ft!  ?  XFM  : '  ft )  ,  XFM2i  5  )  . 


*XF'13<M  t  HU  15 , 61)  ,  HT2  ( ,  NF  2  I  d,fc-> 

»«Hl(ofa.9)fi2tfci..e).H3(6*.  8) _ _ 

*,XcM(e),PF  =  l(>j.6l,FFr2(6,  E).PrF3(8.9»  PFk11i8,9),  PF*2  (?.  3)  ,P«M3<8. 
♦)«  PHIFK  8.  ft  ).  P-<  1^2  (  a.  6)  .  F3(  £  .h)  .  P>-  TFT  1  (  0.6)  ,  PhIFT  2(9  .  3  ) , 

*FHIFT3(3,  9)  ,  OF  Q1 1  E,  £>  ,  QFD  2<  8, 9  )  »  QFD3  l  6 , 9)  ,  HFl  (  £-»  >  ,  HF2  {  6w  )  tr  F3  (( .-4  >j 
•RIH1  (6h»  ,RIn2  (  6U  ,  FIh3(b,  )  .  EXT  RAl  (6,  5)  ,EXT=A2<  E,  9)  •  EXT  Fi  3  ( e»  =)  j 


*, EXTP1( 3) . TXT R2( 5 > , £XTR3( 8>  ; 

WR  IT  E  ( 6.  1) 


READ  *,T0 

PRINT  *,  “INITIAL  V  pCS  IT ICN 


REAO  *»?0 
PRINT  *,  »INIT 


RE*D  *tXjOU 

PRINT  *,  “INITIAL  VELCCITY.  X  I  I  SECTION. 


Rt«D  **  Y  DO  T  C 

pR  INT  ».  “INITIAL  V.-  L  CCITY,  T  LI  SECT  ION 


READ  *t?2GT0 

PRINT  *,  "INITIAL  VE-CCirY,  l  IIRECTION 


XCOTC  = 


TCOTC 


Z  DOT  b 


XDCTc 


IT. 


7  DCT 


I  f) 


isUtM 


E238839S 


'  %::-~l 

t  vti;t  »(  ;  a  _  \015_j  1  -  r  E >  •  .-1-0 


1 


is^tl 


:rHS'\.N'  .L)  -O  7  j  2  1 

re * , ; ( i) , c < 3) ,c(* >  c(5 > 

PRIM  ♦.  "  SPATIAL  NCILc  3  ORREL  A  T  ICN  COL  FFIC I  .NT 

t  •* 

W  • 

PRINT  *, 3l 1) , Gl2)  *3 (3) ,C<  A) ,CC  E ) 

CONTINUE 

-  1 

PRINT  •• 

READ  *,  SIIM-C 

PRINT  *,  "INITIAL  FILTER  SI.-NA  VllGCITY, 

~:~0  *.  ARi 

SIoHF  £•  =  M,SI.KFfi  . 

PRINT  *,  "INITIAL  PIL"!  P  AiPr3‘  RA‘10, 

K _ 0  *,  S I  ;  F2 

arc  =  ”,ap. 

PR  IN  T  *,  "PHS  FILTER  *  T  f'  D  S  P  n  £  R  I  3  NCISF.  ♦ 

READ  *,  RF 

3IGF2  =  ** ,  G  I  F. 

PRINT  *,  ** F I . *  ;  R  Mr  ASU^t-NiM  '(LILT  PVS- 
RE  A  0  *,  SIL-F1C 

Rc  =  "fPF 

FRINT  *.  “INITIAL  R«3  CYNAr.IIS  FCR  FILLER, 

RI-0  ♦,  F IM A<  c 

SIC-F1C  =  ",£I  FlC 

A 

PRINT  *,  "FILTER  HA  X  I  NT  E  NS  I T  f  , 

FI  K„X  C  =  ”  » F I  'M  .» C 

4* 

l 

r 

i 

PARAMETER  V-LJES 

m 

w 

f* 

r 

r* 

. . . 

r 

THE  Hli H  3  rURNM  IS  FUT  II,  MIS  VtRSICN 

Th£  HI j H  S  TURNN  IS  FUT  If.  THIS  VtRSICN 

c 

r 

I _ 

XFQV=8. 

_ YFCV  =  8. _ 

NPIX=8 

_ AT  AUl  =  l4  .  It. _ 

AT AU2=659. 5 

_ g~TAU2=l. /A~ AJ1 _ 

DT  =  i.'3C. 

_ IREF=1 _ _ 

S I GVF*A ;S (SI3MF&) 

_ FIMAX  =  A35  iFUtXIl _ 

C 

£_ _ IN  IT  I  A  L I Z  ■  ~  R  JT  n  ^  CPE.,.  VARI -  FLES 

m 

_ CALL  gtjSil  <756521 _ 

on:  =  i 

_ MP3  a  d _ 

NM3  =  N9IX»*2 

_ NC3=8 _ 

NF32=NF;*NcS 

_ N  FSM2=NrS-  2 _ 

K>  I  G  =  3 

_ RI=l./Rr _ 

3N=SIGM43/Ikax 

_ :F_(5rj.LE  Ci  1 _ 


■  2.*ATA'J2 

:t/ t  Ficn**4) 


fact/ t fac‘ i**3> 
fa:t/ t Fac; 1**2) 


.X3  (  2.  ♦  AT  A  U 1  *  D£  LI  ) 
X3  (rA^*^  LT) 


1.-  LX3  (2.*A7AU2*D£L1) 
OT  *  E  a  3  (  jc-T*FACT2) 


DT*£XP(2.*A7  AU2*  C  LLT ) 


PHIIl.ll 


PHI(2,2)  =  F  i  I  (  1 »  1 ) 

PHK3.3)  =  l  <Pt  AT  AU1*  PELT  ) _ 


PH  1(4*4)  =  EXPi ATAU2*CLLr ) 

PHK4.5I  =QT+=Hl  (  h/U _ 

DHI(5,E)  =  PHl»*.M 

PH  I  ( 6 » 61  =  PHH  3.  3)  _ 


PH  I  (7, 7)  =  PHI'4,M 

PHI(7. iL  =  HI  ■  A .  F ) _ 

PH:<8,S>  =  PHlit-E) 

WAX  IE J  6.  11) _ 

11  FORMAT < X // 2 X,  **7  HE  TRUTH  MODEL  S  T^-TL 


FILL  OUT  DISCRETE  INPUT  MATRIX 


TRANSITION  MATRIX  IS» 


JFJJLL  THE  DC  MATRIX  WITH  VA.UL'S  USIN<T  FXACT  INTEGRAT  ION  . 
SEE  lERCIER'S  THESIS  FOR  DERIVATION 
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O  U  U  J  J  Cl 


Qi?  =  a3*»  2»Q-**2 
PHir(4,6)=L“ 

®HIF  (  3  » £>  )s.  DT 
PH  IF  (  2  »  4 )  *  PT 
PHIP(1,3j=uT 
PH  IF  (1,1)*  l 
PH  Ic  (  2  »  2)  =  1 
PH IF ( 3 1  3 ) -  i 
0HIF(4,4J =1 

i  F(u-K  .AI--A2/A1 

ai=qi: 

A2  =  T3  *  Qo-  ')*>*  G5 
A12=A1*m2 
A1SQsC1**2 
A1 CU3  E  * A i** 3 
A2SQ* A2**2 

PHIF(5, 3)=-<2*Q3*< A12*Q6-A2SC*2*Q2)  ♦A2S3*A1) ♦JT/’ A13 J3E 

pHIF(5 , 4)  r-  -2*Q3*  <-A12*Q5-A2SQ*2*Qa>*DT/A1CUBE 

PHIF(5,5)  =  142*Q3*Q*.*a2*DT/A1SQ 

PHIF(5,6)--2*Q3**2*A2*DT/A1SG 

PHlFd,  3)=-2*0~*0T*(  J12*Q6- A2SQ*2*Q3) /A1CUBE 

PH IF(  5 , 4)  s-  -  (2*  W*  v  -A12*Q5-A2SQ*2*Qh)*A2SQ*A1)*DT^A1:U3E 

PHIF(3,5j  s2*«..*)4  **2*JT/A1SC 

PH  IP ( 5  »  6) - 1-2* A 2  *04* Q3*0T/A 1SQ 

PH Ip ( 7  *7 ) =tXP{ CELT/F  TAU2) 

PH  IF  (?,8)J  PH  IF(7,7) 

□0  5010  1*1,  NFS 
JO  5310  J=1,NFS 
5010  PHIFrd,  JJtPHlF(  J,I) 

fill  in  p*  at  tine  0 

PFP (1, 1) *  25* 

PFP(2,  2i  =  PFP(l,l) 

PFP <3,  3)s-2J:0. 

PPPU,  4)  =PFP (3, 3) 

PFP  (5,  5 )  *  1  Cl -3  •  - .  - .  . 

PFP(5,  5 )  =  1 0 j • 

PFP(7,  7)  =  .2 
PFPC8,  61  =.2 


FILL  IN  3FD  AT  TINE  0 

Qc0  (5, 5)“SIGF1*DT 
QF  0  ( a  ,  a)  =  QFI  (5,5) 

QFO  (7,7)  *  (SIGF2**  c)  *( l.-EXF ( 2.*DELT/FTAU2>> 
Qc  0  (  5 , 6 )  =  QF 0  (7,7) 

C 

C  TIME  LOOP  ETA  ST S  HERE 

C 

5C  TIME  =  TI.1t  ♦  OT 

IF  { TI  ME  .  v»T  »7FI  NALt  GO  TO  99  - 


■; _ CALL  CH3LlSK(Q.  «OP<tMS» _ 

CO  33  1=1, NIS 

j  _ DO  33  Jsl.SlS _ 

|  +  =  rfCRKtJ.I) 

_ SOOD  (  ,  J*5>  -  WCR<(J,I) _ 

[  73  COUTINUi  ‘  . " 

\  _ URITEtf.  35) _ 

['  Tj  PORMAT(///2X.  "!  Hi  CiDLESKY  SQJA  RZ  kOCt  OF  OD  IS»"/) 


..  NOIitnnwlS  Old  t 0  lINOU  M93': 


*  *  1 M  x  e  d 


..  iMlfcd 


(/..ixiaitfw  UOIi  :  T7litdS  -9  X  «:9..  ‘X2///)j.VWfcOd 

- <*-♦;  *9)  BllStf 

3HNI  IN  03  t  £ 


0*0=  (/T+I*6  ‘T-I*fe  )b 


o  r?  o 


tJ7R=: . 

I£  =  5_. _ 

IN  =j. 

AS=  C 


F:MAX=ASS(  FIM4>  C> 
ar=ar: 


r  =  A  :  S  (SISKFU 


: _ PRCVI  3E  CI L' £R  HITh  INITIAL  V.LCC1TV  A  N  0_  PC  £17  I0N_ 

c 

_ RH  0R  =  (  X I  *  «  2*1  G*»2) _ _ 

RA  NC-E=  (RKCR  +  f  C  *  *2 ) 

_ yFo(3)  =  (Zu»x:r~L-x:*?ccTD)/(RH:R*.oa;c2) _ 

RH0R=Sn«T (RHOPi 

_ X FP <  «*)  g  (  RHQR»Yj3T  C-YL,*  (  (X  C*X33 ■  Z  ♦Zj.JJ_C0.TJ  )  /RhOR)  ,  /  (  RANGE*  .  G  J  :  0  £i 

PRINT  RrOP=M,  P-0- RAN3£=  ",  RANT.  E  ,  “  XF^l  3)  =  ",  XFP(  31  , 

J- _ XFP4=“,  y  F3  “  X  PUNS  •• ,  L _ 

RESET  INITIAL  COOIT IONS  F3R  nek  RUN _ 

3  FILTERS  P-D; ( :» =.3333 

Pl  =  .  3333333 _ 

P  2  = .  3333333 

Pl  =  .  3333333 _ _ 

C  FlLTCt  3N£  INI'  I A  L  L I Z T  1 0 f. 

CALL  INinxF3li  XFM1.QFC1,  PFP1,  F  H  I  F 1 ,  a  R  t  S 1  ,  NF  E ) 

_ SI'-Fl  =  1000. _ 

XF°l(3)=XF=m 

_ XF°1(^)  =  XF=>(4) _ 

CALL  PHIFC-M(PH.IF1,PHIFT1,  D<  ,D;LT  ,FTAU2,NFS) 

_ CALL  QF30N(QFJ1,5I3F1,CT.  SI Gc? »  Ol~.LT  .  FT  AU2 ,  NFS) _ 

CALL  PP. JS0 (3F=>1,  NFS? 

r  FILTER  TWC  INITIALIZATION 

r _ 

CALL  INIT(XF>2,  X-K2,0FC2,  PFF2,  PHIF2,XRLS2,NFS) 

_ SIGF1  =  3  L  T  . _ 

XFP2( 3) =XFP( 3) 

_ XFP2(t.)  =  XFr  („  ) _ _ 

CALL  PHIFoM(3nIF?,PHIFT2,  DT  .  CELT.  FTAU2.NPS) 

_ CALL  nF3S^fQrC2.SISFl.CTt  S IG  F?  ,  DLL  7j  F7A  U2.NCS) _ 

CALL  PP. JSO ( 3  F- 2,  NFS) 

C _ 

C  FILTER  HRLL  IN  IT  I ALLI  Z-  T  13  N 


CALL  INI7<  XF=3.  XFN3,  0FC3,  FFP3,  FH  IF3,  XRES3,  NFS) 

SIGF1  g  6  G u  « _ _ _ 

XFp3(3)=XFP(3) 

XFP3  (4)  =  Xp3  <  ■» ) _ 

CALL  PHIFGM(°H:F3»F-jI  F73.  CT  ,  DE  l  T  ,  FT  A  U  2 ,  NFS ) 

£  A  LL  _QF3S  M  9F  C3  f  S  IS  F 1  ,  C  7,  SI  CF2  ,  Q  EL“j  FT  AU2j  NFS ) 


6R 


CO  NT  INU 


C 

F  3 

tim:  =  riN-:  *■  :,t 

PR  IN’  *,  “'HE  =  *•»  T  I  f'L 
iP(Ti;T.:.T.:-:-;f  _>  cc  :c  q- 

c 

_ L _ 

PLRPDkN  T  R  U .  h  MC3LL  SIMULATION 

VTIMC=TIMz-C;/2. 

IF(TIMf-2.C)  £LC,f:C«FuL 

X  03T  =  -liS  3  ii . 

Y  TOT  =  C . 

ZOOT=0. 

X(/--H  =  XG*X:CT»T  I  Mi 


YVEH=Y0 
ZV£H  =  70 


V 

GO  TO  51u 

r 

_ £ _ 

r 

c 

>✓ 

*'?S«S{3S*SS  tSSSfSSf  SftSSSf  SSSt  imSSttStf  *tfSf  SSSSS$S**$ft* 

c 

f 

XDOT  =  -1jCC.*:CC(C.C9?*(VTIN;-2.  )  ) 

YDOT=10c  0.  *5 IK ( C. C98*  f  V T 1  ML-2*  1  ) 

X\ZEH=X0-2C  :C.-1C2C4.*SIN(  0.  D 93  *  (TIME- 2.)) 

YVEh=Y0*lC2:4.M  1«-C0S(C.  G95M*IME-2.)>) 

ZVEH=Z3 

5«  C 

CONTINUE 

RH0Ps(XYEn**24Zv*H*»2) 

PANCE=(  RH0R4I  V;  H**2> 

•IT  (l«l)s  (ZVE-l’XDOT-Xl'O’ZDCDX  (  RHCR*  *  00  0  3  2) 

RH0R=SQRT (RhDF) 

UT  (2,  1M  (  Pi  OR*  YODT-Yvi  h  *(  C>.  VH*  X  DOT  4  Z Vi  H*  ZDOT )  /RHOR)  )  /  '  RA  N.  i 

-JCC02J 

g ATlGt:  =  S3RT  (RAN^E) 


VMAX=SQ  RT  <  xD3T**2  +  YO:T**2  4Z03r*  *  2) /<  RANGE*.  3C0  C2> 

XCtNTR=<  Zt\±l±U]_il •  1)  *bC(  1.  1) _ 

YCfNTR=YCLNTR4UT  (  2,  1)  *tCl  2.  2) 

CALL  NOISE.  MP5,«') _ . _ 

CALL  MM  3  Y  (  7  L  M  P ,  S3Q0»  K  ,  KPS  ,K=>S,  3N_> 

CALL  MM3  Y  ( '  r  MPl ,  PH  I « >.  S»  l>PS»  SPS  ■  ONE  ) _ 

CALL  MA  DD(  XS»  TiK-fK  KF1,NF.',OMi  ,ONl) 

CALL  MM3  Y  (  ~l1P1  .  -  D, UT, NPS jjLi 3Xi_I _ 

CALL  MA00(XS,  XS,TFM>1,KPS,CN£,  DNC) 
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p  r  p  a  c 


CALL  MN3  Y  (  kFll.  »iIFl.  >FP1  (NFSt  NFS'CNZ  ) 
CALL  M'PY  >  >  Fig.  P-iIF?.  VFP2  jNf  Sj_NF_S,  CNF) 
CALL  MM3  Yl  x  F.13.  3-tIF2,  XFF3  .NFS,  N  F  £ , ONL  ) 


FILTERS  C  0  >.  A  R I  NEE  PROPAj  A:  ION 

LAuT "pfTi  l.  JS("  F^yTf7>^  V  ^ jf i  "a  -_I  fTi  .  QF  C 1  * 'CNE 
Call  E  I  ‘  ■  "S(  CJ  Y  3  .  FF  =  ?.  r  -t  if  2._s  -  I  2,  OPC_2t  ONE 

CALL*  PFlir.JS  IF  F»;?,P  ff?,  ^  IF  3,  3*  I  FT  3,  QFrs.cr,-; 


OIL  FCSIUCN  ANE  FILL  Tallin 


XT  c  A 1 ) 
XTFA2) 
XT  RA  ? ) 


C-i  GO  TO  6 


.  =  xS ( 1) ♦* S ( 3) +XS( M  -XFMt « 1) 
=  XS(  2>  «->.S<  6)  S(  7»  -XFM1  i  2) 


ALL  MElSlXFIf  K,*PE  AK,(,tZ,f>) 


search  for  fimax  *.nc  fimin 


if(fima<:  .:-r. )  ro  i  o  59 
fihxo=fihax 


FI  max*  j 
F I  MIN-  C 


DO  55  1=1-  V.* ^ 5 

IF(2(I).  OT. FIM-X)  F I “ A X 


IF<2(I).LT.  FIMIN)  FIMN  =  z  i  II 

FIMAX  =  FIMAV+.  1-*£Ii;vF**2-i.52*c  t- vf+-,35 


FIMAX=  .6*FIMXE+  • 2  *  F I  MAX 
CONTINUE 


PERFORM  MEASUREMENT  U3  C- TE  FO»  THE  FILTER 


INVERSE  COVARIANCE  FORM 

FORM  FILTER  CENTROID  POSITION  AND  FILL  OUT  NON  LINEAR 


ALCJLATE  PARTIAL  SMALL  H  FART IA  L  X 


XPEA  Kl=  X  F M 1 ( 7  I 
YPiAKl  -  X  FM1 ( 5 ) 


xp::ak3=  xfm3(  d 
Y A K3  s  XPM3  (5) 


IF  l  (XFMl  3)  **2«-xFM(4)  ♦*2).  EQ.C.  )  XFH3)  =.  t 
ALL  MEASF(X3E:Kl.YPFAKl,CNE,MFi,Hl.XFMi) 


S  F  (  X  3  L  ’  K  2 
S  F  <  X  3  E  -Kl 


MTi(J,I»  *M1  (I ,  J) 
HT  2(  J*  I)  =  H2CI»  J) 

c  - 

HT  3( J» I)  =n  3  <  I  *  J) 
CONTINUE 

III 


X _ cal;ulh£  :  a  stoilual: _ 

C  RlH=R£S:DJAi.S  =  Z- SMALL  >•■ 

r _ 

call  MA  3  D  {  5  H  1  •  Z  *  H  F 1 .  NM  S ,  0  N  E » 'I  F  S  ) 

_ CALL  KAO  0  (  ~  I  ■)  2  •  Z  .  F  f  2  ■  KMC,  O’,  ’LlILH 

CALL  MA30v-H3.Z,PF3.  NNS,  CVI?  <  F  S i 


c  covariance  jp.'trr 


CALL  PP.  LE !  PrPl  ,  P*-I  pi  ,h?l  1,MP5,N:S.FFM1) 
CA  L  L  PP- US  <  FiiiUib  1  F?  .  "T  2_  -  2 ,  N  P  3_ ,  N :  S  •  E  F  T2 ) 
CALL  PP.'JS(  p- P3,  =  FI  F3  .  AT  3  ,-.3,  <J  F  S  ,'nNS  ,  PFM3) 


C  STA " £  UPDLTi 

C 

CALL  MM=>YlEXTRl.iTl,PlFl,  KF3.  MRS  ,  CL.  ££  > 

CALL  MFPYLEXl  R2,HT2,RIh2,  NFSt^MStCNE) 

CALL  MM^Y  <  £  XT  S3.  hT3.  RIP3.  NFS.NMS.CNE) 

C  EXTR1=  HT  *  { Z-SMiLL  H ) 

r  IF  IFL-C  =1.0  0-0  TO  121  FJC  7H.-\  PILFER  CNLY 

D  It  NOR*  J  FOP  RIGHT  NOW.  NSEC  TC  P'JN'  OMb  FILTER  ,-T  A  TIM. 

IFLAt=a 

.  CALL  XF3U3  (  X‘ PI.  PFP1.  EXTR1.  NFS  •  IFLAt.  RI.  XFM1) 

CALL  XFPUPlX«f2,PFP2,EXlR2.NFS. I F LAG , R I , XFM2 J 

CALL  XF=>UP(  XpF3.  FFF3,  FXTR3.NFS.  I  PL AG , R I . X F 1 3) 

CALL  CCHDPRr  {  F£  OMCl ,  PFM.l,  HI,  HT  1  ,  NKS,  NFS , RN .  Rlhl) 

CALL  COMDPR£(  FD  3  NC2 . prM  2. H? . HT  2 , NFS, NFS, RN. RIH2) 

CALL  CONOPRLT  f  D  GNL3  ,  PFM3,  H3,-»f  3 , NMS, NFS, RN, RIH3) 

_ I£LAtJL=> _ : _ 

IFLAC-  2=0 

I  FLA  C- 3=3 

C  IFLAS  s  1  IF  -OW=s.  _  OUNCTN3  OF  FI,  F2,  P3,  ORDA  OCCURS 

C  IFLA j  NOT  C.I'JC  USED  AT  TiIS  TIME 

FSUU  =  RDCNDl*yl*F:ONr2*P2+F03N03*P3 

CALL  PROF  (PI,  FSUM.FCCNCl.  IFLA31> 

CALL  PRDc(  =  2,  FSUM.FCONC2,  IFLAJ2) 

CALL  PR0B(P3.  FSUN.F0  0NG3.  IFLA33) 

PRINT  "  1=  "rl,"  F2=  ",  P2»  **  P3=  ",F3 

.  00  211  1  =  1. N“S  _  , 

XFM(I)=<FM1<I)*:»1+XFM2(I>*P2KFM3(H*P3 

XPP(I)  =  XFPl  (  I)  *Fl+XFF2(  I>  *P2*XFF3(I  )  *P3 

’ll  CONTINUE 

X  °  i_"  A  K  =  XS  (  1)  *  X  S  (  3  )  *  XS.->  -XFM(1) 

Y p A  K  =  XS12>  «■  XSIE)  +  X  S  (  7 )  -X.FM(2) 

IF(AfcS(<Pf  IK)  .  •  7.  3.  *t  EFR3  *EIS1£  )  GO  TO  131 

P££K)  .ST.3.»*.SPRD*SI3'1S>  00  T  0  102 

r 

i 
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S  A '/  L  ( 1 2 1 

=  P-M3,3> 

3  A  VE  ( 1 3 

=  PrP( ?, 2) 

3  A  Vet  1-41 

=  F*  P.1,,4,) 

HRITl(o) 

SA  V: 

♦RUN", 


GO  TO  135 


-l/1?  PRINT  »,  "CG5  T  TRACK.  Y  GhANNEi,  NLAS  CALLE.C  ",IMLAS,"  tI  Hi 
*  RUN”*  L» “  TIM£  =  ",TIM:  ' 


l"c  JO  110  J=l,  1, 


S4  VE ( J) 1 0 • 
WRITL<9>  SAVE. 


GO  TO  136 
95  CONTINUE. 


STOP  "FINISH 
-  N  J 


QFG(I,  J»  =  j. 
PFP(I,J)  s  C. 
CONTINUE 
RF  TURN 


EMC 


*f /I 


DIMENSION  PFPi NFS* NFS 


FILL  IN  Pt  -I  TIM£  L 

Fpp:(  i7i»  =TiT 

PFP(  2.2)  sf-rr  (  i .  i  > 


♦  31  =  20  u j  . 

.£)  =PTP(  3 •  3 ) _  _ 

silo. 

JL.1LC-* _ 

=  .2 
= .  2 


RETURN 

END 


SUi^CuTTNE  ,eMJ NUS 


FT  N  o. 2: 


.  F-MlS  JStPF  M.RPP-  PHIF  .  PHIFT  .  CFQ .  UN 


L'IMp  N3IDN  3FM(  NFS  .NFS  J  ,  P-Fi  NPJ  .  NCS)  ,  P^IFf  NFS, NFS)  -  PH  I  FT  1  NFS: 

8QcJt  NF$,  NFS  Ui  iT  R  £  (  N  F  E ,  N5  S  J _ _ 

* , P  FPOLDl 8.6),  PPFP (S. F ) 

_jaiiEG£.£j?-^i _ _ _ 


RETURN 
£N  *“ 


£  DPI =2 


SUNROUTIN;  pxO.(P,FSUH. FI OND, I  FLAG) 


FTN  4.8+E23 


P  =  .01 
IFL£G=1 


CONTINUE 
RETURN 


NF  52  =  NFS*N"S 

CULL  SHifliPFJ'jPrr-  :.f,,rojSr;  ^) 

*!*.*>’ 

::k-i  =  t. 

C  ALL  L1M/2  F  l  3F"i,  r.FL.  r.FL,£  >"  RA,  I  C:  T  •  W  K  A  c 
L'X~Pi  =  3  (  7  I  -  >  -  1 


DO  6f  1=1, NFS 
DO  6?  J=1,SF3 


PFPdt  J)  =PPP(  I.  J)  *RI  +  L  X  T  F  A  <  I  >  J  ) 

FS  CONTINUE _ _ 

PFP=  °(TIO-l  =  ->7  *  k-1  *  -i  +  PlTI-)-l 
AT  =  j 


CALL  LIX(/2F(:,FF,VJFS,‘<FS,£X1  RA,  I  DC T, WK A*D A , I ER) 
DO  70  1=1, NFS 


00  70  J=1,NFS 
PF?(  I,  J>  =  r  XT  -  A  <  I.  J) 


73  CONTINUE 

00—13C  1=1. N 


PFP=s ( T I ♦) 

return _ 

DM  Z 


DO  56  J=l, NFS 
PHlFTtl.  J)=F-)IF1  J.l) 


p.E-U»N 

FNO 


IF'Ml,?!  =CT*»w*  SICrl'  6. 

.of (  i  s  i  r- c  1  /  £ . 

Or  “  (  2,  2)  =Qr:(  111 
0F-!(  2,-1  l-3l 

OF  J(2,o>  =  D=T(  1  5  I 
0FCI  3, 11  =0r:(  1  3) 


QFD£«,3I  =0F^(  ?.  71 

RETURN 


J'J 


FT N  n.P+r?3 


OIMENSI3N  *  F=>  l  NFS  )  ,  PFP(NFS.  NFS  )  ,  LXT  R~  (  NFS) 
v  r 


CALL  MM’YOX,  PFP.EXT  RA,  NF  S«  NFS  •  ONE) 
=  BM  Tt)  *  HT  * 


Hitt 


DXDXT (I, J) =0<  i  II  *  D  X  (J) 

CO  NT INUi _ 

oxoxr*  -  x(ti->;  *  (x<ti*j 

TR  XXTC  =  ?  Q  G  . _ 


"rRXXT  =  0, 

DO  79  1*1. NFS 


TRXXT=7\XX7  OX:XT  (1,1) 
MANIND=  3 


TR XXT  =  ,5*rFX<r 3 ♦, 2*TS  XXf 

CALL  HA30(XF?,.X.  XF*,  NFS,  ONE.J'.K) 

XFP=  <(!!♦)=  c  (  T I  ♦)  *  R-l  *  rT  *  (Z 
RC"  URN  _ 


END  7 » 


-  X(TI-))T 


-  SNhLL  H  ♦  X(TI-) 


_ ’ll  jfeftil  T  HttUuh?;  ■  I 
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l  yn  DC_“ ,  h  .  5, ,  NF  :.,j£s ;  _  _  __ 

CALL  MfPT  t  -iPF.S-ii  ,  FPFv,  M,  K;:S,  V  F  S  M'S  i 
C  A  LL  HA  PC  (  -  J.  .  P  N  «  MM  S  .  MMS  .  ON") 


1=1, V^S 


A ( I,  J)  =  D,  C 

VERS  l I, J) =Cr ' 


COMTINU 


T  A  Kr.  OIACONAl  OF  AJ  Mi  Tr  IX  -  S  KLPRF  SE  NT  AT  IVE  OF  THE  -HOLE 
MATRIX  T0  £  J 'JIG  A  feL  X  oU  MATRIX  INVERSION 
00  6  1  =  1,  N  “  S  '  "  . . .  . '  . . ' 

A(I,I)=AJ(I,I) 


COMTINU 


deta=i. 

_  AINVERS  =  A**» -1) 


DO  9  1=1, NMS 


T 

CONTINUE 

DO  10  J=1.NM3 

XRESTd,  J)=XF.E5l  J) 

> 

1** 

CONTINJS 

F(Tl  =  l/<t2»3.1L15  =  **M/2)  *A3C  (  A  J)  *♦,?*£  yP(-> 


(-)=(-• 5 *X REST *( A J**-l )♦ XR£S )  •  K=«  CF  FILTERS 
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1  SUG^DUTINE  HEAS  ( XPE  AK , YF EA  K,ISUE>,  Z  ,R) 

CO HMON/FLIR/  XFO V,  YF  C7  , I  MAX , NPIX , SIGHS , SIGMFTSaGM A3,  SIGFLR, RF 

*  ,AS3RD,FIMAX,UT<  2,  1)  .iMEASt  AR,SIGVF,PL2P(64t2>  ,'/MA<,  SIGHFC 

*  t’p  AU3Z  0*  kA  NSE*  SI 3PVF  ,CSTHt  S  NTH 

=3  REAL  IMAX  _ 

. . 01  MENS  ION  2  (3,  6)  ♦  R i  6% ,64r~,Hn'6'4Vi >  t  W t(6¥TlT 

ZMIH  =0. 

.  . " '  SI  SP </  =  ST  GHS^ITA  KG E 0  /F:  AN  GE  ' 

PLVF_=SCRTMJT<1,  1L**2+UT<2,  1>**2> 

10  SNTHejr  (2,13/PLVIL  ‘ 

CSTH=UT<1, 1J/PLVEL 

*  - - STG\Z*(lV+(7rsrFR0=r^)*PL'VEr/\rKTJXl'r»^TUFV - 

I  -  (  NPIX *ISUE } 

X3TV~r~TSUB^»2 - 

XI  NCR  =  XFOV/FLOAT  {I) 

y  i  nc  r-»~YFTJV7FnjjrmT  : — : 

X  =  •!. *XFQV/2».  >  XI  NCR/ 2. _ _ 

XI  =  X 

CA  LI.  NOISE  (64»  wTj "  .  1  “ 

CALL  HKPY(»CvR,Hlt  5L,  64,  1) 

MN=0  ‘  .  . T 

DO  23  K= lt NPIX 

oo  13  j*rr,-KPix-" .  •  .  . . 

TDTA_  '  s'~CT.  . . .  . . . . 


IH 


lSNTH 


XM  =  < 

YV  s  Y 

QD  13  N=  1, ISUB 
TYN-TPEAT 
YSN7  H=  {  YN-YPEA  K) 

"00” '5  "ff  ”=T7I5”UEs  . . 

APGS^^sYCSTH-tXN-XPEAO^SNTH 
~V>  SS7bTXR=7P  EAKT*  G’STH  ♦YSTffiT 
A2G=-{  (ARGSV//SIGV)  **  2*  t  ARGSP  V/SIGP  V )  **  2)  *~5 

TOT  AT~s  TO  TAL+LXP1 7TRTT)”  *TtfSTX - 

XN  =  X  +FLOAT  <M)  *XINCR 


■?9" 


10 


TTD'flTTfrOE  • — 

YN  s  f*  FLOAT  <N)*YINCR 

XNsT  . 

CONTINUE 

■zrcrj  JrTOT  AL'/FLOTTn  DTV)' 


C 

T 


ADD- B7»CT<ffR0UtTD_  A  NO  '  FLTR"NTC1"SE— E'OTTT'ZERirHESTr 


*0 


"GO”  TO  30” 


~sr 


TP  (E T3"-tKTEQ70 .”) 
GAUS3=C. 

- Tjo“imxE-irrz - 

GA  US3  =  GAUSS+RA NF (DUH1) 

TltmONTINUET - 

F=  (GAJ$S-6,)*SIGFLR 

vTd^r>T- . 


30 


zee,  jt 

Z{K,J) *Z<K,J)*WDIMN,  1) 


- - M 


ituUMilw 
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Mbl  VW<*te*»HiH*rtH>  V*u AV-.« 


- :  “'i?  { z (K ,  j>  irr^ZMiwiKiMszTKr^r 

X  =  ♦FLOAT (ISUB)*XI NCR 

IF  CONTINUE  . 

Y  =  f  -  FLOAT  <ISUB>*YINCR 

x=»xo 

2'G-  CONTINUE  "  ' 

\  IMEASs  IMEAS-*1  _ 

IF  CZNIN.EQ.G.)  RETURN 
00  25  I«=l,N3IX 
00  25  J=  1*  N^IX 

Z<I,J>  =  ZIItJ>-ZHlN*.l _ 

‘  25  CONTINUE 
RETURN 
END'  'T 


I 

~ . 5  JrEOJTTK 


0  J  TT7IE~”S  h  I FT 


74/74  OFT  = i 


| - 


suaROJriNE  shift i  a  ,  s  »is»  n) 

. .  01  MENS  1 Q-N  A  (Nr) ,  B  <  N) 

C  A:  INPUT  ARRAY,  3*  OUTPUT^  ARRAYjs 

c . ‘  . . 

NN=N  _  _ 

Do  33  0  'I=I;N  .  "' 

3CC  3<I)=0. 

~c  r  TEST  for  LEFT"  OR  '  RIGHT  SHIFT 

IF  (IS. LE « 3 i  GO  TO  IDO _ 

C . EXECUTE  RIGHT  SHIFT 

RM*i*I S  _  _ 

H'sN-IS  "  .  . 

CALL  3HIFTA«(i)  ,B  (MM)  ,M,NN> 

- S'5“TC3~20d  ‘  ' 

C  EXECUTE  LEFT  SHIFT 

ICO  MM=1-Is 

Ns  N+1 3 

- CA  L'L ~S  HlFT'Al  AFhH  F7E  CiTTHTWJ 

ZCC  RETURN _ _ _ 

END . 


FTN  4.3*52S 


= ARRAY  SIZE, IS=AN,T  OF  SHU 


subroutine  shifta 


7  l,/ 7  u  OPTcl 


FT N  4.  3*<528 


_  SUBROUTINE  SHI  FT A ( A , C, M, N) 
'  Ul HENS  I ON  A  (  N)  ,  E  ( N ) 

00  13  3  K-  1 »  R _ _ 

100  ' S  C  K)  =  A(K) 

RETURN 


ENO 


I 


3'J?R  JUTINE-  ME. ASF 


74/74  OPT  *  i 


FTM  4,3*528 


JS'JORDJ  T INE  MtA  SF  ( X  PE  AK,  YPE  AJ<j  ISU6,  Z ,H, XFM, 

CO  MM3N  / F L I R/  X FO V  ,  Y F  0 V  , I MA X  ,  NP IX ,  SI GMS  , S  IGi  S 13 1 A 3, 
* j„A_S PRO»£IMAX,UJ<  2,  1)  » I hE  A S,  AR,  SI G VF,PL2P(  64,2 J_» ■  VNAXj 
*, RAN3EC,  RANGE,  SIGPVF  ,CSTH,SNTH 
DIMENSION  Z  (6,8), H( 64,8) ,XFM(8) 

RiAL'  I  MAX 
ZM  IN  =  0. 

PLVE.a  S  CRT  tXF  M(  3)  **  2*XF M  (  4  )  **  2) 

S  N  T  H  »  <FM(4) /PLVEL 
CSTH=  XFM(  3)  /PLVEL 
SIGPrfFeSIGVF/AR 


=  <NPIX*ISUB) 
13 IV  *  ISUB**2 


XI N-?  =  X  F  CV/FLOA  TCI) 

YINCR  *  YFOV/FLOAT (I) 

‘  -l.*XFOV/2.  *  XI  NCR/ 2. 
t-CV/Z.  -  YINCR/2, 

X3  s'  X  -  - 

DO  2)  K=1,NPIX 

Ii"s  k  . . 

DO  15  J=  1 ,  NP I X 


SIGFLR* 

SIGMFO 


YN  = 

DO  13  K'=i  » ISUb 

YC SH=  i  Y;«-Y PEA  K)  *3ST H  "  .  ” ~ . .  . 

Y3MH*  (Yn-YFEAK)  *SNT  H 

00  5  *  =1, ISUE  . . 

ARCS3/  sY'CST rr-(XN-XPEAK  )*SN7  H 

ARGS.'s  lX;-«-XP£A<)"*CSTH+YSNTH . .  ~ 

ARG*-( I ARGSV/SIGVF)  **2+ ( A RG SPV/S IGPVF) **2 > *, 5 
PARTa  EXP  URG)  *FI  MAX 
TOTA.  =  TOTAL+PART 

SUM1  *  SUM1  .♦  PTRTM  A^GSV*CSTH/SIGVF**2-AR£SPV»SNH/SIGPV'F*' 
SU  M2  a  SUM2  ♦  PA  RT*  ( A RGSPV*  CSTH/SIGPVF**2+ARGSV*S NTH/STGVF*’ 


XN  -  X  + FLOAT <M)*XINCR 
5  CONTINUE 

YN  =  Y  -FLOAT  (N)  "*  Yl  NC  R  " 

XN  *  X 

wcosironor - - * - 

Z1K,  J)  a  TOTAL/FLOAT  (IDIV) 

I F  (  3 1 3  MF  if*  G T •  5 • )  "  GO*  10  16  . . "  . 

PL2P(K  +  (  J-_l)*3,l)s  PART*  (-(  ARGSPV/SIGVF)  **2*AP.) 

PL  2P  (  <  J-  i)*e  ,  2)  a  p  ART*(  ( A  RGS V **2  +ARG SP V ** 2*AfC** 2 )  /  S I G  VF  ***• 

CONTINUE 

'  1 F  ( Z  ( K’TJ )  .  ITi'Z  MI  N )  Z  M  IN  a  Z  ( KTJ  )T  . 

H ( NUM, 1)  =  SUMl/r L OA T ( IDIV )_ 

H( NUM, 2)  =  SUM2/FL0AT(IDIV)  ‘  . . 

H( NUN,  3) "0  * 


rn  NU.Tt  O)  -u  . 

H(NIH,  7)~  H(NUMtl) 

HtNUM,  6)  =  H  {  N0M72 )  "  ~ 

X  «  <N  «-FLOATCSUB)*XINCR 
NUM  «  Nll.1  "♦  NPIX 

15  CONTINUE  "" . . 

T  a  T  -  FLOAT  ( ISUS  )•  VI  NCR 
x'ix'd 

20  CONTINUE 

IFTZil  N.  ECU  DV)  RETORN 

_ DO  25  I  =  1,N3.IX_ 

DO  2  5  J= 1 »  NPIX  ~  . 

__Z(I,J)  s  Z  <I,_J)-ZMIN  +  .i_ 

25  Continue  -  ' 

RETURN 

- ENTD . . 


£  MAOD 


74/7A 


OPT=  1 


FTN  4.3  +  528 


SUBROUTINE  MAJODt  C,  A  ,  &  ,  J,  K,  I  FLAG) 
Cl  MENS  I  ON  A  ( J»  K)  ,9  ( J,  K )  ♦  C  <  J  ,  K>  ~ 
I“(I*_AG.EO.i)  SO  TO  6 
■"  sc  5  v  =  i,  J 
00  5  Msi,k 

. ~C  '  N ♦  *i  1  's'  A(M,M>  -~B(  N,M) . 

5  CON’INJE 

return 

6_D0_  13  Ns  I*  J 

00  '13'  ><*:,  A  ' 

C(N,<)  =  AC.-ifN)  +  S<N,K) 

To  conTnue"  ' 

RETURN 


END 


toot 


74/7*  OPT  si 


"FTN  4*8  +  528 


FUNCTION  DOT  (NR,A,B) 

. Dimension  ai  d  /BCD 

DOTS). 

DO  1  1  =  1,  N  R 
1  DOT  s  COT  +A < I) *3 { I ) 

. .  'RETURN 

END 


74/74  0P~T  =  1 


FTM  4*  d *528 


SJSROU 

l  _ 

5 

in 

15 _ 

IT  . . 

25__ 

33 

55 _ 

-3 . 

45 

S3 


TI  NE-  LOCATE 


SU3PDJTINE  LOCATED,  RD, REF,  XR,XTR,T,TDtC,_E,KK,KD)_ 

01  MENS  ION  R(  KK) ,  RC ( KD )  ,T ( KK ) *  C ( 3) ,  f  0  (  KC) , REF  C KK) 

CALCULATE  THE  TARGET  CENTROIO  POSITION _ 

. KT-« 

KS=KO  _  _ _ ; _ 

CALL'  C ENT R ( 7  , XT ,  <T ) 

CALCULATE  A  ROUGH  ESTIMATE  OF  TARGET  POSITION  OFFSET 

XTRsXT-XR  ~  '  '  . . 

C  _  ROUND  0----  ANSWERTO  NEAREST  INTEGER  _ _ 

"  " .  "CALL  RNDFTXTR,  MARK) .  . “  . . 

COMPUTE  TARGET  DENOMINATOR 
GALiTRDEN  (T,7D,KT,  KS) 

DO  120  JJ=1,3 
100  C( JJ) *  0. 

J=  2 

IS  “MARK-2 
N=  « 

~c  STAR-  MAIN  LOOP  HERE  . .  ♦♦♦♦♦♦^♦♦**.**V***** 

200  CONTINUE _ _ _ 

j=J*l  . . 

IS=ISH 

"IF  ' 1 3  »  Gi •  0  •  QR.VISVl  t»  “3 )  ~GO~  TO  525 
C  SMIrr  *ErE~ENCE  DATA 

CALL  5-IrT  <R,R£F,  I  S»  N)  . . '  . " '  ' . . 

CALCULATE  fJE  SQUARE  OF  ThE  CORRELATION  COEFFICIENTS 

. “sum*;"."' *  .  . . 

00  30  0  JJ=1,  N 

“  SU^sSJ-'+T'!  JJ)*e.EF'C  JJ)  . .  . 

200  CDN’IN’UE 
CA-Sj-t**’ 

OENQM  =  TD(Nj-I_S>  *RD(N*IS ) _ _ 

I-  (DESOA'.lT  .l.E-6)  PP.INT»,"N=SN»"iSs,*»ISt:,TDc“,TO(')-iS), 
*“R0  =  “»  R3JN+IS)  ,"SUM= ",  SUM,  "MARK="»  MARK 
C(  J)  siOt/DENOM 
Ic ( 0 ( J ) • G7  *1  • )  CCJ)  =  1. 

e  test"to  settf  c  (2)  hss'  been  computed 

I" (C<2) .LE.3.)GO  TO  20C 
I- (CC2) .GE.C(l) )  GOTO  400 
C  MISALIGNMENT,  SHIFT  LEFT) 

'  ‘  C  (  3)  =3  (  2) 

c<2)  =  :m _ _  _ _____ 

MARKsMARX-l 

‘  ■  T3'=Ts-3  ‘  . . 

GO  TO  200__  __ _ _ 

VDO  CONTINUE  "  . .  . .  "  / 

IF (C(3) . L  E  .0 .)  GO  TO  200 

- - IF  (0(3)  .LT.T<2>)  GO  TO  500  .  . ”  r"V"‘ 

C  MISALIGNMENT  SHIFT  RIGHT 
C( 1)=C (2) 

C(2)=G(3) 

. . MAR  K*M  ARK+T 


toe  CONTINUE 

~EW"OF~H&rN  LOOP  SECTION 


0MPU7E  TARG  ET-RCF ERE MCE  PRECISION  OFFSET  ESTIMATE 


OX  =  Q. 

IF  (C ( 1 )  •  C'4«  S«~C  »OR«  C(  e)  •EQYC  •  0»OR»C  {3)  •  EQ.  OV)  'GOTO  55  0 
0N=2.*(i./C(i)  »1./C<3)  -2  *  /C  ( 2 ) ) 
mffN.  GTVO.)  GO  TO  W 
525  EslOOQ. 

so  n  too  '  . . 

550  PRINT** **N A  RNI NG  **********  CORRELATION  COEFF  =*  0  .  " 

’  ON =2.  M  27*  C  (  2)  -C  (  1  )-CT3T) 

DX  =  (C(3)-C(1))/DN 

- £— “rrOXTTHA  RKV*DX  ’ 

SO  TO  700 
~f02“CO'NTTN'JE 

OX=(l./C<i)  - 1  •  /  C  (  3 ) ) / ON 
‘E=  F  LOTTTM  A  RX  f+ 0X~ 

7  CO  RETURN  __  _ ■ _ _ 


_S'J3R0U  TINE  MOUT(  A,  NR,  NC) 
DI  MENSION  A  (NR, NC) 

00  13  1*1, NR 


HRITE(S,5>  <A(I,  J)  ,J=1,NC) 
FORMAT  <2X,  MG13.  7,3X  )> 


CONTINUE 

RETURN 


ENO 


S'JBRD U T 1 .41:  CORPORA,  T  A,  li ,  J1 ,  XCORR ,  YCORF  , CX,  C  Y) 

01  PENSION  RACi,  Jl) ,  TA  (1 1,  J 1 ) , R  <  6 )  ,T  ( *  ) ,  TARGT6)  •  2  C  3)  ,R0(15) « 
1  TD(1=)  f  hlEFCBI  _ _  ,  . _ 

PERFORM ‘ ROW  CORRELATION 


CX  =  .  95 

CY=.95 . . 

1  COUNTsQ. 

SUMX=3.~  . 

N=  Jl 

R=2*N"-i  . 

EE=i.  £-8 
OD'TTC  Q“TT=T7Xi 
00  11C0  JJ=1,J1 
R  (  JUT  "  ="  >7 ATI  ITHT  «■ E  E 
T  (  JJ)  =  TA<II,JJ)+EE 

TIC  0  "CONTINUE  .  ' 

CALL  ROEiKR,  RD,N,K) 

CA  LL  0  z  N"  ft  <  R,X  R,"Nr 
CALL  LOCaTE(R,RD,REF,XR,XTR, 
IF  <C(2J  .GT.CX)  GO  TO  21C3 “  “ 
TA«?5(II)  =  1000. 

GO"’TD"  223  0'  .  . 

2 ICO  CONTINUE 

~"TAR3<  1 1)  =  E  ’ 

2200  CONTINUE 
1200"  "CONTINUE 

DO  33  G  C  I=1,J1 

1F-(T ^  -  (I  y  -.  FQ.  iu CO.)  GO"  TO" 
SiJMX*SUMX-4-T^RG(I) 

COUNr=COUNT+l.  . 

3CCC  CONTINUE 

1  eTu  3TfNT7c  Qi'S7  nr  "GO"  10"“T3  S"C"0“ 
XC  ORR*  SUMX/G  CUNT 


rcTC, £,*!,<) 


PERFORM  CORRELATION  ON  COLUMNS 


2_COUNT=0 
SrJMV*Q". 


- - 

DO  423  0  JJ=1,J1 
DO-RfKTTTIaiTlI 

Rt  II)  =  ft  A  (II«  JJ)  +EE 

- rnir"s  "TATirrJjy  ->EE- 

4100  CONTINUE 

- CAUr'ROrN  ( FTfRDiKjK)  — . . 

CALL  CENTk(R,XR,N) 

- CffOTTOHATErrRVRD'TTTEFrXRrXI 

IF  (C(2)«GT .CY)  GO  TO  3100 
TARGTJJ)  =~1'000."  ' " 

GO  TO  32C0 


.crrrNv 


3ica  continue  — 

TARG(JJ)  =C 
3203  CONTINUE  " 
h2C0  CONTINUE 


iCQ 


3530 


5000 


DO  \0CC  1=1,11 

£?  (T4R.S(I)  .CQ.13C0.)  GO  TO  ^00  0 
SUMY  *  SUMY  ♦  TARS (I) 

COUNT: COUNT+1. 

CONTINUE 

IF  (COUNT.  EQVo.'O)  GO  TO  50dQ" 
YCORR  s  SUMY/COUNT 
YCOPN  =  -YCORK 

RETURN  _  _ 

CONTINUE  .  ' . ' 

CX=C<- .025 


IF  (C<  •  Lf  .  0 .5  )  PRINT*,-**********  CX<£.S  *********** 

GOTO  1 _ _ 

CONTINUE  “  .  . ~~ . . 

CY=CY-.025 

IF  ( C  if”.  LT  •  0.5 )  PRINT*,  “ ' 

GO  JTO  2 _ 

END 


•  *****  ***** 


cr<c. 5 


************ 


r- 


S  J2ROUTINE  R  j[  N 


T*t/7*»  OPT  =  l 


FTN  ..3*52fl 


_1  _  .  SUBROUTINE  POL  NR,D,N,K)._ 

C  HIS  SUfcROUTI  N£  COMPUTES  THE  CORRELATION  DENOMINATOR 

...  _ <L  ....  __  FORJhE  REFERENCE  FUNCTION. 

C  Rs  INPUT  REFERENCE  ARRAY .  ~  ' 


H — 


C_ 

"c 

C 


. . D_=  OUT  PUT  _  DE  NOMINATOR  A  P.  RAY  _  _ 

N  IS  THE  INPUT  ARRAY  DIMENSION  AND  ALSO  7 HE  3 J 7^ J  T  ARRAY 

_ MARKER.  POSITION. 

K  is  THE  OUTPUT  A  FRAY  DIMENSION 
01  HENSION  R < N)  , D ( K ) 

SUM  *  0.  ~ 


(/>, 


AD-AU5  509  AIR  FORCE  INST  OF  TECH  WR1 BHT “PATTERSON  AFB  OH  SCHOO— ETC  F/«  17/0 

ALTERNATIVE  DYNAMICS  MODELS  AND  MULT1FLE  MODEL  F1LTER1N0  FOR  A  — ETC(U) 
DEC  01  F  M  FLYNN 

UNCLASSIFIED  AFIT/tC/EE/BlD-21  _  ML 


( 


>'Jr 

’  OUTnmSTTOLE 

S*<  74/74  0PT=1  FTU  4*3<-52d 

l 

SUBROUTINE  CH0LE3K  (A,S,N) 

DIMENSION  A <T)  , S ( 1 3 

NOIMsN 

O 

NDIM1=N*1  .  . 

TOLsl.E-6 

MR  =  0 

MN=N*NDIM 

TOL1=0. 

DO  1  1  =  1,  NN,  NDIM1 

n 

1 

R= ABS (A  (U  J 

IF  (R.CT-TQLliTOLlsR 

T  0  Cl  =  7  0U*l.t-12 

11  =  1 

15 

□0  33  I=i,N 

IMl  r  1-1. 

5 

0 0"5_JJ=I,Ny,NDl  M~ 

StJJ)  *-3. 

10  =  1 1* IM1 

R=  A ( ID ) -DOT CIM1, S(II).S(II)  ) 

2D' 

I F  ( A  3  5  ( RTTlTTTTO L* A  ( I D )  +T 0CTJ7  GO  TO  50 
!-(  =  )  15,50,20 

15 

M3.  =  -i  ■" 

4  r  r  £  f  3,  looo)- 

25 

. Inrcru" 

FORMAT  TKltO  TO  FACTOR  AN  INDEFINATc.  MATRIX  ) 

return 

20 

SU'D)'  =  S2SPTTPT 

*o-u5*i 

1~  (X»E3*N)  RETURN  . 

L=II«-NCIM 

'23  " 

00  15  J  J=  L  »N  N,  NDl  M 

I J- J J* I Ml 

25 

50 

S(IJ»  =  rA(ru)-DOT(lMl,GflH  ,S(JJ)  >)/S(ID) 

II  =  II  +  NDIM 

25 

RETURN 

END 

— rr=  r  iu>i  .T-nrePY  74774  rM  v,  stst* 

- - : - — \ - : 

1  SUBROUTINE  MMPY(C,A,E,K,M,N) 

.  01  ME  NS'  ION  0  IK,  N)  ,  A  ( K  »  M )  »  B  ( M  ,  NV 

DO  1  1  =  1, K _ _  _  _ _ _ 

"  "00  1  J  s  i  ,  N  ■  '  . . 

5  C(I,  J)  =0 _  _ _ _ _ _ 

.  T~CDNTINJt  .  . .  .  ■  ’  ' 

DO  5  LSI.  K 

. . .  DO'  5  J^T,T1  "  “*  ^ 

00  5  1=1, M  _  _ 

.J  C(L,  J)  S  C(I,JI  ♦  (A  (L,IJ*B  tl.J) ) ' 

5  CONTINUE 

. .  . RETURN 

END  _ • 


84 


r,  o 


SU5R3 J  TINE  CEN7R(A,X,J> 

.  THIS  SUBROUTINE  COMPUTES  THE  CENTROID  OF  THE  ARRAY  A 

THE  OUTPUT  VARIABLE  ISIX,  AND  THE  ARRAY  DIMENSION  ISJJ 

DIMENSION  "ACJ) . . " . . .  " . "" 

SUM  «  0. 

s  =  OV"  "  . '*  . .  : 

x  =  1. 

~dd  roT'i=i,"j  :  . .  "• 

SUM  s  SU.H  ♦  FLOAT  (I)  *AtI) 

ledssSVATi)’'  -  "  '  ----- 

IFCS.LE.G.J  GO  TO  20  0 

- tzrsu  h/s  : - 

200  RETURN 

- rN  o - 


ju'Tnrr'fiiHOF  "7  4/ 7-4  opr=i  ~  '  •  fym  v.  s*5~?6 


s,je=3,jt:  ;e  rndf  <  a  ,  k> 

c  3JB ROUTINE  TO"  ROUNDOFF  A',TO  ‘.NEAREST  INTEGER,* 

C  _  _  _ _ _ _ _ _ 

'k=rrix  <  a>  "  - 

B=A-< 

- GO'  TO'  TOD  ~ . 

K-sK  +  i 

'1'wC  'RETURN  . .  . . . . 

END 


u  rmr  rfwRm  74/7  s  oft  » 1 


FTN  4»  8 <-52 8 


SUBROUTINE  MWRITE  (A ,N  *M,  ID  IM) 
DIMENSION  A  (IDIMVi) 

PRINT  M 

0(5''3?  0  "  1\- 17 N  . . . 

WRITE  (6,340  )  (A(I4,U4),U4=1,M) 

2-+L - FORMA T  •  ( IX', 1 6 ( F7 . 4 ?i X )  ) 

353  CONTINUE 

'PRINT'  V  " 


-  \ 


APPENDIX  C 


Performance  Plots  for  the  Brownian  Motion  Filter 

This  appendix  contains  the  plotted  outputs  of  the  performance 
of  the  BM  filter.  Three  trajectories  were  simulated  and  eight  plots 
are  included  for  each  case.  They  are: 

—mean  error  ^  1  sigma  of  the  filter  estimate  of  the  'x'  and  'y' 
target  position.  These  plots  were  used  for  checking  the  mean  bias 
error  and  rms  position  error  in  general. 

—mean  error  +  1  sigma  of  the  filter  estimate  of  the  * x '  and  'y' 
target  velocity.  These  plots  were  used  to  check  any  mean  bias  errors 
and  rms  velocity  errors  in  general. 

— true  and  filter-indicated  standard  deviation  of  the  'x'  and  'y 
position.  These  plots  were  used  in  tuning  the  filter  to  the  various 
trajectories  simulated. 

—true  and  filter-indicated  standard  deviation  of  the  'x'  and  'y 
target  velocity.  These  plots  were  used  in  tuning  the  filter  to  the 
various  trajectories  simulated. 
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HERN  ERROR  ♦-!  SI COR 
TARGET  OTNAMCS 
X  CHANNEL 
FLIR  FOV 


X  CHANNEL  OYNflMICS  ERROR  lS/N=i2.5) 


Figure  C-l  20  g  Q*600  Performance  Plot 
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5.6i 
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VELocrrr 
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0.80  1.60  2.40  3.20  4.00  4.80  5.60 
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ACTUAL  VS.  FILTER 
SIGMA 


5.6 


MEAN  EltIWR  4~1  SlCrtA 
TARGET  OTNAMICS 


Y  CHANNEL  DYNAMICS  ERROR  C5/N=12J5 
Figure  C-5  20  g  Q*600  Performance  Plot 
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i 


5.6 


ACTUAL  VS.  FILTER 
SI  COR 
Y  CHANNEL 


FILTER  VS.  RCTURL  SIGMR  PLOT  ( S/N  =12.51 


Figure  C-6  20  g  Q*600  Performance  Plot 


08*0 


ACTUAL  VS.  FILTER 
SIGMA 
T  CHANNEL 


5.61 


mean  error  +-i  sigma 

TARGET  DYNAMICS 
X  CHANNEL 


X  CHRNNEL  DYNAMICS  ERROR  [S/N=12.S) 
Figure  C-9  10  g  Q=600  Performance  Plot 
05 


5.6 


ACTUAL  VS.  FILTER 
SIGt1fl 
X  CHANNEL 


FILTER  V5.  ACTUAL  SIGMA  PLOT  (S/N  =125) 
Figure  C-10  10  g  Q=600  Performance  Plot 
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9'S 


00 

00*2  OO’O 

00*2- 

00 ’fr-0 

S13XId 

X  CHANNEL  VELOCITY  ERROR  CS/N=12.5) 
Figure  C-ll  10  g  Q-600  Performance  Plot 
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5.6' 


(CAN  ERROR  *-l  SIG(1A 
TARGET  DYNAMICS 
Y  CHANNEL 
FLIR  FOV 


Y  CHHNNEL  DYNAMICS  ERROR  CS/N=12.-S] 
Figure  C-13  10  g  Q=600  Performance  Plot 
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0.00  0.80  1.60  2.40  3.20  4.00  4.80  5.6 


OO’V 


OO’fr- 


OO*0-D 


00  '8 


00*0 

S13XId 


Y  CHANNEL  VELOCITY  ERROR  CS/N=12.5| 
Figure  C-15  10  g  Q®600  Performance  Plot 


(CAN  ERROR  +-1  S1GHA 
PARGET  OTNAMCS 
X  CHANNEL 

flir  rov 


X  CHANNEL  OYNRHICS  ERROR  IS/N=12.S) 

Figure  C-17  2  g  Q-600  Performance  Plot 
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5.6i 


RCTURL  VS.  FILTER 
siom 
X  CHANNEL 
VELOCITT 


FILTER  VS.  flCTURL  SIOHR  PLOT  IS/N  =12.5) 
Figure  C-20  2  g  Q-600  Performance  Plot 
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2*40  3.20  4.00  4.80  5.60 

TIME  (SEC) 


Ot'O  03’0  OO’O  03’0-  Ofr’O 

S13XId 

Y  CHANNEL  OYNRMICS  ERROR  (5/N=12.5] 

Figure  C-21  2  g  Q-600  Performance  Plot 
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vELocirr 


Y  CHANNEL  VELOCITY  ERROR  tS/N=12.5) 

Figure  C-23  2  g  Q=600  Performance  Plot 
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TIME  (SEC). 
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Figure  C-24  2  g  Q=600  Performance  Plot 
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9  *9  08’*  00*V  03*£  OVZ  09*1  09*0  OCTtL, 

1 _ I _ L  till  I 


09*  l 

08*0  00*0 

OB'O- 

09*  I 

913XId 

X  CHRNNEL  DYNAMICS  ERROR  (S/N=12.5) 
Figure  C-2S  2  g  Q=150  Performance  Plot 

m 


9  *S 


— I  >-  -J  > 

i  t  g  £ 

o  z 

«s  o  a.  ac 
o  J  r  - 
K  U  U  J 
K  >  U. 
U  X 


00*01 

00*3  00*0 

00*S- 

00*01-° 

913X1 d 

X  CHANNEL  VELOCITY  ERROR  15/N=12.5| 
Figure  C-27  2  g  Q=150  Performance  Plot 
113 


5.6 


flerURL  VS.  flLTOt 
SIGMA 
X  CHANNEL 
VELOCttr 


FILTER  VS.  FICTURL  SIDHfl  PLOT  IS/N  =12-5) 
Figure  C-28  2  g  Q*150  Performance  Plot 
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5.6i 


HERN  ERROR  4-1  St CUR 
TRRCET  OrNRtltCS 
T  CHANNEL. 


Y  CHANNEL  DYNAMICS  ERROR  IS/N*2.5) 


Figure  C-29  2  g  Q=150  Performance  Plot 
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APPENDIX  D 


Perfornance  Plots  for  the  Constant  Turn  Rate  Filter 

This  appendix  contains  the  plotted  outputs  of  the  performance  of 
the  B!'  filter.  Three  trajectories  were  simulated  and  eight  plots  are 
Included  for  each  case.  They  were: 

— nean  error  ^  1  sigma  of  the  filter  estinate  of  the  'x'  and  'y ' 
target  position.  These  plots  wer o  used  for  checking  the  nean  bias 
error  and  ms  position  error  in  general. 

— mean  error  *  1  sigma  of  the  filter  estinate  of  the  'x'  and  'y * 
target  velocity.  These  plots  were  used  to  check  any  rean  bias  errors 
and  rns  velocity  errors  in  general. 

—  true  and  filter-indicated  standard  deviation  of  the  'x'  and  *y' 
position.  These  plots  were  used  in  tuning  the  filters  to  the  various 
trajectories  simulated. 

—  true  and  filter-indicated  standard  deviation  of  the  'x'  and  * y 1 
target  velocity.  These  plots  were  used  in  tuning  the  filters  tc  the 
various  trajectories  simulated. 

Plots  indicating  the  convergence  of  the  variance  over  20  ”onte 
Carlo  runs  are  included  in  the  20  g  and  10  g  ^=.?0C  plots.  These 
plots  we~e  used  in  determining  how  many  "onte  Carlo  simulation  runs 
were  required  in  order  to  get  meaningful  results.  ."11  the  convergence 
plots  for  both  the  Brownian  motion  and  CTP  filters  were  very  similar 
in  appearance  and  information,  so  only  a  few  are  included  as  samples. 
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